function [IMG_ED, EdgDet_map] = EdgDirInterpolate( IMG )
% CFA Edge Directed Demosaicing.

IMG = double(IMG);
IMG_SIZE = size(IMG(:,:,1));

IsYsizeOdd = mod(IMG_SIZE(1),2);
IsXsizeOdd = mod(IMG_SIZE(2),2);

%VAR_THRESHOLD, the threshold of the variance that if the local variance of
%point(x,y) is higher than this threshold, then (x,y) is considered as a
%edge point, and a covariance-based interpolation will apply to this point.
% Set VAR_THRESHOLD = -1 will make the covariance-based interpolation
% applied to the whole image ( NOTE: It might take a long time to process
% whole image with covariance-based interpolation!!)
VAR_THRESHOLD = 100;

%Halfwindow_size, the 1/2 of the local variance window size. For
%Halfwindow_size = w, it creates a (2w+1)x(2w+1) window, therefore the
%boundry of the image of a width w will not be able to apply
%covariance-based interpolation. A bilinear interpolation will be
%automatically applied.
Halfwindow_size = 2;

%Edge Detection map. 1 if pixel (x,y) is considered as a edge.
EdgDet_map = zeros( IMG_SIZE );
for y = Halfwindow_size+2:IMG_SIZE(1)-Halfwindow_size-1
    for x = Halfwindow_size+2:IMG_SIZE(2)-Halfwindow_size-1
        IMG(y-Halfwindow_size:y+Halfwindow_size, x-Halfwindow_size:x+Halfwindow_size, 2);
        [tmp1 tmp2 result] = find(IMG(y-Halfwindow_size:y+Halfwindow_size, x-Halfwindow_size:x+Halfwindow_size, 2));
        varG = var( result );
        EdgDet_map(y,x) = ( varG > VAR_THRESHOLD );
    end
end
clear tmp1, tmp2;
%First reconstruct those 'x' point in R/B channel:
% R   R
%   X
% R   R

%In this step, Halfwindow_size = 2n and Halfwindow_size = 2n-1 is the same
%in R/B channel, so for convenience we always use odd half window size
%here.
win_size = Halfwindow_size - ~( mod(Halfwindow_size,2) );

IMG_tmp = IMG;
%R Channel
for y = 2:2:IMG_SIZE(1)-1
    for x = 2:2:IMG_SIZE(2)-1
        if (~EdgDet_map(y,x) || y < win_size+2 || y > IMG_SIZE(1)-win_size-2 || x < win_size+2 || x > IMG_SIZE(2)-win_size-2 )
            IMG_tmp(y,x,1) = ( IMG(y-1,x-1,1) + IMG(y-1,x+1,1) +IMG(y+1,x-1,1) +IMG(y+1,x+1,1) )/4;
        else
            tmp = IMG(y-win_size-2:2:y+win_size-2, x-win_size-2:2:x+win_size-2,1);
            tmp_size = prod(size(tmp));
            Crow1 = reshape(tmp, 1, tmp_size);
            tmp = IMG(y-win_size+2:2:y+win_size+2, x-win_size-2:2:x+win_size-2,1);
            Crow2 = reshape(tmp, 1, tmp_size );
            tmp = IMG(y-win_size+2:2:y+win_size+2, x-win_size+2:2:x+win_size+2,1);
            Crow3 = reshape(tmp, 1, tmp_size );
            tmp = IMG(y-win_size-2:2:y+win_size-2, x-win_size+2:2:x+win_size+2,1);
            Crow4 = reshape(tmp, 1, tmp_size );
            C = [ Crow1 ; Crow2 ; Crow3 ; Crow4 ];
            tmp = IMG(y-win_size:2:y+win_size, x-win_size:2:x+win_size,1);
            tmp_size = prod(size(tmp));
            y_vec = reshape(tmp, tmp_size, 1);
            alpha = ( C * C' )^-1 * ( C * y_vec );
            Neighbors = [ IMG(y-1,x-1,1) IMG(y+1,x-1,1) IMG(y+1,x+1,1) IMG(y-1,x+1,1) ];
            IMG_tmp(y,x,1) = Neighbors * alpha;
            %Sometimes it gives a negative result... My solution is: if it
            %is negative, I will apply bilinear interpolation.
            if (IMG_tmp(y,x,1) < 0 || IMG_tmp(y,x,1) >255)
                IMG_tmp(y,x,1) = ( IMG(y-1,x-1,1) + IMG(y-1,x+1,1) +IMG(y+1,x-1,1) +IMG(y+1,x+1,1) )/4;
            end
        end
    end    
end

if ~IsYsizeOdd
    for x = 2:2:IMG_SIZE(2)-1
        IMG_tmp(IMG_SIZE(1),x,1) = ( IMG(IMG_SIZE(1)-1,x-1,1) + IMG(IMG_SIZE(1)-1,x+1,1)  )/2;
    end
    if ~IsXsizeOdd
        IMG_tmp(IMG_SIZE(1),IMG_SIZE(2),1) = IMG(IMG_SIZE(1)-1,IMG_SIZE(2)-1,1);
    end
end

%B Channel
for y = 3:2:IMG_SIZE(1)-1
    for x = 3:2:IMG_SIZE(2)-1
        Neighbors = [ IMG(y-1,x-1,3) IMG(y+1,x-1,3) IMG(y+1,x+1,3) IMG(y-1,x+1,3) ];
        if (~EdgDet_map(y,x) || y < win_size+2 || y > IMG_SIZE(1)-win_size-2 || x < win_size+2 || x > IMG_SIZE(2)-win_size-2 ) %For those points which are not edge, use bilinear interpolation
            IMG_tmp(y,x,3) = mean(Neighbors);
        else                % For edge points, use covariance solution.
            tmp = IMG(y-win_size-2:2:y+win_size-2, x-win_size-2:2:x+win_size-2,3);
            tmp_size = prod(size(tmp));
            Crow1 = reshape(tmp, 1, tmp_size);
            tmp = IMG(y-win_size+2:2:y+win_size+2, x-win_size-2:2:x+win_size-2,3);
            Crow2 = reshape(tmp, 1, tmp_size );
            tmp = IMG(y-win_size+2:2:y+win_size+2, x-win_size+2:2:x+win_size+2,3);
            Crow3 = reshape(tmp, 1, tmp_size );
            tmp = IMG(y-win_size-2:2:y+win_size-2, x-win_size+2:2:x+win_size+2,3);
            Crow4 = reshape(tmp, 1, tmp_size );
            C = [ Crow1 ; Crow2 ; Crow3 ; Crow4 ];
            tmp = IMG(y-win_size:2:y+win_size, x-win_size:2:x+win_size,3);
            tmp_size = prod(size(tmp));
            y_vec = reshape(tmp, tmp_size, 1);
            alpha = ( C * C' )^-1 * ( C * y_vec );
            IMG_tmp(y,x,3) = Neighbors * alpha;
            %Sometimes it gives a negative result... My solution is: if it
            %is negative, I will apply bilinear interpolation.
            if (IMG_tmp(y,x,3) < 0 || IMG_tmp(y,x,3) > 255)
                IMG_tmp(y,x,3) = mean(Neighbors);
            end
        end
    end
end

if IsYsizeOdd
    for x = 2:2:IMG_SIZE(2)-1
        IMG_tmp(IMG_SIZE(1),x,3) = ( IMG(IMG_SIZE(1)-1,x-1,3) + IMG(IMG_SIZE(1)-1,x+1,3)  )/2;
    end

    if IsXsizeOdd
        IMG_tmp(IMG_SIZE(1),IMG_SIZE(2),3) = IMG(IMG_SIZE(1)-1,IMG_SIZE(2)-1,3);
    end
end

%Then Reconstruct those 'x' points in R/G/B channel
%   R
% R x R
%   R

%R/B Channel
for y = Halfwindow_size+2+1:IMG_SIZE(1) - Halfwindow_size -2
    IsYOdd = mod(y,2);
    initial_x = 1+mod( y+Halfwindow_size,2 ) + Halfwindow_size + 2; % initial_x is even if y is odd.
    for x = initial_x:2:IMG_SIZE(1) - Halfwindow_size -2
        Neighbors_R = [ IMG_tmp(y-1,x,1) IMG_tmp(y,x-1,1) IMG_tmp(y,x+1,1) IMG_tmp(y+1,x,1) ];
        Neighbors_B = [ IMG_tmp(y-1,x,3) IMG_tmp(y,x-1,3) IMG_tmp(y,x+1,3) IMG_tmp(y+1,x,3) ];
        if ~EdgDet_map(y,x) %For those points which are not edge, use bilinear interpolation
            IMG_tmp(y,x,1) = mean(Neighbors_R);
            IMG_tmp(y,x,3) = mean(Neighbors_B);
        else                % For edge points, use covariance solution.
            %R/B Channel: C matrix and Y vector preparation
            C_R = zeros(4, 2*Halfwindow_size*(1+Halfwindow_size) );
            C_B = zeros(4, 2*Halfwindow_size*(1+Halfwindow_size) );    
            y_vec_R = zeros(2*Halfwindow_size*(1+Halfwindow_size),1);
            y_vec_B = zeros(2*Halfwindow_size*(1+Halfwindow_size),1);        
            counter = 1;
            for yc = - Halfwindow_size : Halfwindow_size
                IsYCodd = mod(yc,2);
                for xc = (~IsYCodd)-Halfwindow_size:2:Halfwindow_size
                    %Row 1 of C: Upper neighbors.   
                    C_R(1,counter) = IMG_tmp(y+yc-2,x+xc,1);
                    C_B(1,counter) = IMG_tmp(y+yc-2,x+xc,3);
                    %Row 2 of C: Left neighbors.
                    C_R(2,counter) = IMG_tmp(y+yc,x+xc-2,1);
                    C_B(2,counter) = IMG_tmp(y+yc,x+xc-2,3);
                    %Row 3 of C: Right neighbors.
                    C_R(3,counter) = IMG_tmp(y+yc,x+xc+2,1);
                    C_B(3,counter) = IMG_tmp(y+yc,x+xc+2,3);
                    %Row 4 of C: Bottom neighbors.
                    C_R(4,counter) = IMG_tmp(y+yc+2,x+xc,1);
                    C_B(4,counter) = IMG_tmp(y+yc+2,x+xc,3);
                    %Y vector
                    y_vec_R(counter) = IMG_tmp(y+yc,x+xc,1);
                    y_vec_B(counter) = IMG_tmp(y+yc,x+xc,3);                
                    counter = counter+1;
                end
            end
            
            %R/B Channel: Reconstruction
            alpha_R = ( C_R * C_R' )^-1 * ( C_R * y_vec_R );
            alpha_B = ( C_B * C_B' )^-1 * ( C_B * y_vec_B );
            IMG_tmp(y,x,1) = Neighbors_R * alpha_R;
            IMG_tmp(y,x,3) = Neighbors_B * alpha_B;
            
            %Sometimes it gives a negative result... My solution is: if it
            %is negative, I will apply bilinear interpolation.
            if (IMG_tmp(y,x,1) < 0 || IMG_tmp(y,x,1) > 255)
                IMG_tmp(y,x,1) = mean(Neighbors_R);
            end
            if (IMG_tmp(y,x,3) < 0 || IMG_tmp(y,x,3) > 255)
                IMG_tmp(y,x,3) = mean(Neighbors_B);
            end
            
        end
    end
end

%G Channel:
for y = Halfwindow_size+2+1:IMG_SIZE(1) - Halfwindow_size -2
    IsYOdd = mod(y,2);
    initial_x = 2 - mod( y+Halfwindow_size,2 ) + Halfwindow_size + 2; % initial_x is even if y is odd.
    for x = initial_x:2:IMG_SIZE(1) - Halfwindow_size -2
        if ~EdgDet_map(y,x) %For those points which are not edge, use bilinear interpolation
            IMG_tmp(y,x,2) = ( IMG_tmp(y-1,x,2) + IMG_tmp(y,x-1,2) +IMG_tmp(y,x+1,2) +IMG_tmp(y+1,x,2) )/4;            
        else                % For edge points, use covariance solution.
            %G Channel: C matrix and Y vector preparation
            C_G = zeros(4, 2*Halfwindow_size*(1+Halfwindow_size) );
            y_vec_G = zeros(2*Halfwindow_size*(1+Halfwindow_size),1);
            counter = 1;
            for yc = - Halfwindow_size : Halfwindow_size
                IsYCodd = mod(yc,2);
                for xc = (~IsYCodd)-Halfwindow_size:2:Halfwindow_size
                    %Row 1 of C: Upper neighbors.   
                    C_G(1,counter) = IMG_tmp(y+yc-2,x+xc,2);
                    %Row 2 of C: Left neighbors.
                    C_G(2,counter) = IMG_tmp(y+yc,x+xc-2,2);
                    %Row 3 of C: Right neighbors.
                    C_G(3,counter) = IMG_tmp(y+yc,x+xc+2,2);
                    %Row 4 of C: Bottom neighbors.
                    C_G(4,counter) = IMG_tmp(y+yc+2,x+xc,2);
                    %Y vector
                    y_vec_G(counter) = IMG_tmp(y+yc,x+xc,2);
                    counter = counter+1;
                end
            end
            
            %G Channel: Reconstruction
            alpha_G = ( C_G * C_G' )^-1 * ( C_G * y_vec_G );
            Neighbors_G = [ IMG_tmp(y-1,x,2) IMG_tmp(y,x-1,2) IMG_tmp(y,x+1,2) IMG_tmp(y+1,x,2) ];
            IMG_tmp(y,x,2) = Neighbors_G * alpha_G;
            %Sometimes it gives a negative result... My solution is: if it
            %is negative, I will apply bilinear interpolation.
            if (IMG_tmp(y,x,2) < 0 || IMG_tmp(y,x,2) > 255)
                IMG_tmp(y,x,2) = mean(Neighbors_G);
            end
        end
    end
end

IMG_ED = uint8(IMG_tmp);
% imshow(IMG_tmp);
% imwrite(IMG_tmp, 'test.tif', 'tif' );