function xc = bayer2rgb(xb, M, N, method)
%BAYER2RGB Summary of this function goes here
%   Detailed explanation goes here

    % Initializes the struct that's going to store the RGB image
    xc = zeros(M, N, 3);
    
    % Initializes helper variables
    bayerPatternDimY = size(xb, 1);
    bayerPatternDimX = size(xb, 2);
    
    % Calculates the distance between two grid points for both axes
    gridPointsStepLengthY = (bayerPatternDimY - 1) / (M - 1);
    gridPointsStepLengthX = (bayerPatternDimX - 1) / (N - 1);
    % Calculates the coordinates of the grid points for both axes
    gridPointsCoordinatesY = (1:gridPointsStepLengthY:bayerPatternDimY)';
    gridPointsCoordinatesX = (1:gridPointsStepLengthX:bayerPatternDimX)';
    
    if (strcmp(method, 'nearest'))
        % Determines the indeces of the even rows of the original image
        % that are closest to each ordinate of the new image grid
        gridPointYmod2 = mod(gridPointsCoordinatesY, 2) >= 1;
        nearestEvenRow = floor(gridPointsCoordinatesY);
        nearestEvenRow(gridPointYmod2) = nearestEvenRow(gridPointYmod2)+1;
        nearestEvenRow(nearestEvenRow > bayerPatternDimY) = ...
            nearestEvenRow(nearestEvenRow > bayerPatternDimY) - 2;

        % Determines the indeces of the odd rows of the original image that
        % are closest to each ordinate of the new image grid
        nearestOddRow = floor(gridPointsCoordinatesY);
        nearestOddRow(~gridPointYmod2) = nearestOddRow(~gridPointYmod2)+1;
        nearestOddRow(nearestOddRow > bayerPatternDimY) = ...
            nearestOddRow(nearestOddRow > bayerPatternDimY) - 2;

        % Determines the indeces of the even columns of the original image
        % that are closest to each abscissa of the new image grid
        gridPointXmod2 = mod(gridPointsCoordinatesX, 2) >= 1;
        nearestEvenCol = floor(gridPointsCoordinatesX);
        nearestEvenCol(gridPointXmod2) = nearestEvenCol(gridPointXmod2)+1;
        nearestEvenCol(nearestEvenCol > bayerPatternDimX) = ...
            nearestEvenCol(nearestEvenCol > bayerPatternDimX) - 2;

        % Determines the indeces of the odd columns of the original image
        % that are closest to each abscissa of the new image grid
        nearestOddCol = floor(gridPointsCoordinatesX);
        nearestOddCol(~gridPointXmod2) = nearestOddCol(~gridPointXmod2)+1;
        nearestOddCol(nearestOddCol > bayerPatternDimX) = ...
            nearestOddCol(nearestOddCol > bayerPatternDimX) - 2;

        % Determines the indeces of the rows (even or odd) of the original
        % image that are closest to each ordinate of the new image grid
        totalNearestRow = round(gridPointsCoordinatesY);

        % Determines the indeces of the columns (even or odd) of the
        % original image that are closest to each abscissa of the new image
        totalNearestCol = round(gridPointsCoordinatesX);

        % Closest neighbors for red and blue can be determined by observing
        % the Bayer pattern
        xc(:, :, 1) = xb(nearestEvenRow, nearestOddCol);
        xc(:, :, 3) = xb(nearestOddRow, nearestEvenCol);

        for currentRow = 1:M
            for currentCol = 1:N
                if ((mod(totalNearestRow(currentRow), 2) == 0 && ...
                        mod(totalNearestCol(currentCol), 2)) ~= 0 || ...
                        (mod(totalNearestRow(currentRow), 2) ~= 0 && ...
                        mod(totalNearestCol(currentCol), 2) == 0))
                    % This point of the original image doesn't contain
                    % information about the green colour
                    
                    % Calculates the vertical displacement of the two
                    % diagonal lines that cross this point
                    b1 = totalNearestRow(currentRow) - ...
                        totalNearestCol(currentCol);
                    b2 = totalNearestRow(currentRow) + ...
                        totalNearestCol(currentCol);
                    
                    % Calculates the ordinate of the two diagonals at the
                    % abscissa of the current point (currentCol)
                    y1 = gridPointsCoordinatesX(currentCol) + b1;
                    y2 = -gridPointsCoordinatesX(currentCol) + b2;
                    
                    % Initializes temporary coordinates of the point which
                    % will be selected as the closest one
                    fixedNearestRow = totalNearestRow(currentRow);
                    fixedNearestCol = totalNearestCol(currentCol);
                    
                    % Checks the relative position between the point and
                    % the two diagonals
                    if (gridPointsCoordinatesY(currentRow) >= y1 && ...
                            gridPointsCoordinatesY(currentRow) >= y2)
                        fixedNearestRow = totalNearestRow(currentRow) - 1;
                    elseif (gridPointsCoordinatesY(currentRow) < y1 && ...
                            gridPointsCoordinatesY(currentRow) < y2)
                        fixedNearestRow = totalNearestRow(currentRow) + 1;
                    elseif (gridPointsCoordinatesY(currentRow) >= y1 && ...
                            gridPointsCoordinatesY(currentRow) < y2)
                        fixedNearestCol = totalNearestCol(currentCol) - 1;
                    elseif (gridPointsCoordinatesY(currentRow) < y1 && ...
                            gridPointsCoordinatesY(currentRow) >= y2)
                        fixedNearestCol = totalNearestCol(currentCol) + 1;
                    end
                    
                    if (fixedNearestRow < 1)
                        % Fix for the marginal first row case
                        xc(currentRow, currentCol, 2) = ...
                            xb(1, fixedNearestCol - 1);
                    else
                        xc(currentRow, currentCol, 2) = ...
                            xb(fixedNearestRow, fixedNearestCol);
                    end
                else
                    xc(currentRow, currentCol, 2) = ...
                        xb(totalNearestRow(currentRow), ...
                        totalNearestCol(currentCol));
                end
            end
        end
    elseif (strcmp(method, 'linear'))
        % Calculates helper variables
        flooredCoordinatesY = floor(gridPointsCoordinatesY);
        flooredCoordinatesX = floor(gridPointsCoordinatesX);
        
        % Calculates the ordinate of the upper couple of nearest points
        % that will be used in the interpolation for the blue colour
        upperBlueY = flooredCoordinatesY;
        upperBlueY(mod(upperBlueY, 2) == 0) = ...
            upperBlueY(mod(upperBlueY, 2) == 0) - 1;
        upperBlueY = upperBlueY + 2;
        
        % Calculates the ordinate of the upper couple of nearest points
        % that will be used in the interpolation for the red colour
        upperRedY = flooredCoordinatesY;
        upperRedY(mod(upperRedY, 2) ~= 0) = ...
            upperRedY(mod(upperRedY, 2) ~= 0) - 1;
        upperRedY = upperRedY + 2;
        
        % Calculates the abscissa of the left couple of nearest points that
        % will be used in the interpolation for the blue colour
        leftBlueX = flooredCoordinatesX;
        leftBlueX(mod(leftBlueX, 2) ~= 0) = ...
            leftBlueX(mod(leftBlueX, 2) ~= 0) - 1;
        leftBlueX = leftBlueX + 2;
        
        % Calculates the abscissa of the left couple of nearest points that
        % will be used in the interpolation for the red colour
        leftRedX = flooredCoordinatesX;
        leftRedX(mod(leftRedX, 2) == 0) = ...
            leftRedX(mod(leftRedX, 2) == 0) - 1;
        leftRedX = leftRedX + 2;
        
        % Determines the indeces of the rows (even or odd) of the original
        % image that are closest to each ordinate of the new image grid
        centerPointRow = round(gridPointsCoordinatesY);

        % Determines the indeces of the columns (even or odd) of the
        % original image that are closest to each abscissa of the new image
        centerPointCol = round(gridPointsCoordinatesX);
        
        % Adds a padding to the original image, replicating the last two
        % rows and columns of each egde
        xbPadded = [xb(:, end - 1) xb(:, end - 2) xb xb(:, end - 2) ...
            xb(:, end - 1)];
        xbPadded = [xbPadded(1, :); xbPadded(2, :); xbPadded; ...
            xbPadded(end - 2, :); xbPadded(end - 1, :)];
        
        lowerInterpPointBlue = ...
            ((leftBlueX + 2 - gridPointsCoordinatesX) / 2)' .* ...
            xbPadded(upperBlueY + 2, leftBlueX) + ...
            ((gridPointsCoordinatesX - leftBlueX) / 2)' .* ...
            xbPadded(upperBlueY + 2, leftBlueX + 2);

        upperInterpPointBlue = ...
            ((leftBlueX + 2 - gridPointsCoordinatesX) / 2)' .* ...
            xbPadded(upperBlueY, leftBlueX) + ...
            ((gridPointsCoordinatesX - leftBlueX) / 2)' .* ...
            xbPadded(upperBlueY, leftBlueX + 2);

        xc(:, :, 3) = ...
            ((upperBlueY - ...
            gridPointsCoordinatesY) / 2) .* ...
            lowerInterpPointBlue + ...
            ((gridPointsCoordinatesY - ...
            upperBlueY + 2) / 2) .* ...
            upperInterpPointBlue;
        
        lowerInterpPointRed = ...
            ((leftRedX + 2 - gridPointsCoordinatesX) / 2)' .* ...
            xbPadded(upperRedY + 2, leftRedX) + ...
            ((gridPointsCoordinatesX - leftRedX) / 2)' .* ...
            xbPadded(upperRedY + 2, leftRedX + 2);

        upperInterpPointRed = ...
            ((leftRedX + 2 - gridPointsCoordinatesX) / 2)' .* ...
            xbPadded(upperRedY, leftRedX) + ...
            ((gridPointsCoordinatesX - leftRedX) / 2)' .* ...
            xbPadded(upperRedY, leftRedX + 2);

        xc(:, :, 1) = ...
            ((upperRedY - gridPointsCoordinatesY) / 2) .* ...
            lowerInterpPointRed + ...
            ((gridPointsCoordinatesY - upperRedY + 2) / 2) .* ...
            upperInterpPointRed;
        
        for currentRow = 1:M
            for currentCol = 1:N
                if ((mod(centerPointRow(currentRow), 2) == 0 && ...
                        mod(centerPointCol(currentCol), 2) == 0) || ...
                        (mod(centerPointRow(currentRow), 2) ~= 0 && ...
                        mod(centerPointCol(currentCol), 2) ~= 0))
                    % This point of the original image DOES contain
                    % information about the green colour
                    
                    % Calculates the vertical displacement of the two
                    % diagonal lines that cross this point
                    b1 = centerPointRow(currentRow) - ...
                        centerPointCol(currentCol);
                    b2 = centerPointRow(currentRow) + ...
                        centerPointCol(currentCol);
                    
                    % Calculates the ordinate of the two diagonals at the
                    % abscissa of the current point (currentCol)
                    y1 = gridPointsCoordinatesX(currentCol) + b1;
                    y2 = -gridPointsCoordinatesX(currentCol) + b2;
                    
                    % Initializes temporary coordinates of the point which
                    % will be selected as the center
                    fixedCenterPointRow = centerPointRow(currentRow);
                    fixedCenterPointCol = centerPointCol(currentCol);
                    
                    % Checks the relative position between the point and
                    % the two diagonals
                    if (gridPointsCoordinatesY(currentRow) >= y1 && ...
                            gridPointsCoordinatesY(currentRow) >= y2)
                        fixedCenterPointRow = centerPointRow(currentRow) - 1;
                    elseif (gridPointsCoordinatesY(currentRow) < y1 && ...
                            gridPointsCoordinatesY(currentRow) < y2)
                        fixedCenterPointRow = centerPointRow(currentRow) + 1;
                    elseif (gridPointsCoordinatesY(currentRow) >= y1 && ...
                            gridPointsCoordinatesY(currentRow) < y2)
                        fixedCenterPointCol = centerPointCol(currentCol) - 1;
                    elseif (gridPointsCoordinatesY(currentRow) < y1 && ...
                            gridPointsCoordinatesY(currentRow) >= y2)
                        fixedCenterPointCol = centerPointCol(currentCol) + 1;
                    end
                    
                    xc(currentRow, currentCol, 2) = ...
                        tiltedInterp(gridPointsCoordinatesX(currentCol), ...
                        gridPointsCoordinatesY(currentRow), ...
                        fixedCenterPointCol, fixedCenterPointRow, xbPadded);
                else
                    xc(currentRow, currentCol, 2) = ...
                        tiltedInterp(gridPointsCoordinatesX(currentCol), ...
                        gridPointsCoordinatesY(currentRow), ...
                        centerPointCol(currentCol), ...
                        centerPointRow(currentRow), xbPadded);
                end
            end
        end
    end
end


function value = tiltedInterp(xw, yw, xc, yc, xbPadded)
    % Calculates the vertical displacement of the main diagonal that
    % crosses this point
    bw = yw - xw;

    % Calculates the abscissas of the points of intersection
    x1 = (yc + xc - 1 - bw) / 2;
    %x2 = (yc + xc + 1 - bw) / 2;
    
    % Calculates perpendicular distance of point w to the line connecting
    % the lower point and the left point
    dist = sqrt((x1 - xw) ^ 2 + (x1 + bw - yw) ^ 2);
    
    % Calculates the length of the upper part of the line connecting the
    % points
    h1 = abs(sqrt(((xc - 1 - xw) ^ 2 + (yc - yw) ^ 2) - dist ^ 2));
    
    lowerLeftInterpPoint = ...
        ((sqrt(2) - h1) / sqrt(2)) * ...
        xbPadded(yc + 2, xc - 1 + 2) + ...
        (h1 / sqrt(2)) * ...
        xbPadded(yc + 1 + 2, xc + 2);

    upperRightInterpPoint = ...
        ((sqrt(2) - h1) / sqrt(2)) * ...
        xbPadded(yc - 1 + 2, xc + 2) + ...
        (h1 / sqrt(2)) * ...
        xbPadded(yc + 2, xc + 1 + 2);

    value = ...
        (sqrt(2) - dist) / sqrt(2) * lowerLeftInterpPoint + ...
        dist / sqrt(2) * upperRightInterpPoint;
    
    
    % Tried to rotate the neighbors and then interpolate. It didn't
    % work well but I left it in here anyway.
    
    % Rotates the nearest points by 45 degrees
%     x1 = (xc - 1) * cosd(45) - yc * sind(45);
%     y1 = (xc - 1) * sind(45) + yc * cosd(45);
%     
%     x2 = (xc + 1) * cosd(45) - yc * sind(45);
%     y2 = xc * sind(45) + (yc + 1) * cosd(45);
%     
%     lowerLeftInterpPoint = ...
%         ((x2 - xw) / (x2 - x1)) * ...
%         xbPadded(yc + 2, xc - 1 + 2) + ...
%         ((xw - x1) / (x2 - x1)) * ...
%         xbPadded(yc + 1 + 2, xc + 2);
% 
%     upperRightInterpPoint = ...
%         ((x2 - xw) / (x2 - x1)) * ...
%         xbPadded(yc - 1 + 2, xc + 2) + ...
%         ((xw - x1) / (x2 - x1)) * ...
%         xbPadded(yc + 2, xc + 1 + 2);
% 
%     value = ...
%         (y2 - yw) / (y2 - y1) * lowerLeftInterpPoint + ...
%         (yw - y1) / (y2 - y1) * upperRightInterpPoint;
    
    % Also tried averaging between the four neighbors which actually
    % produces a better image... So I just left this in too.

%     value = (xbPadded(yc + 2, xc - 1 + 2) + ...
%         xbPadded(yc + 2, xc + 1 + 2) + ...
%         xbPadded(yc - 1 + 2, xc + 2) + ...
%         xbPadded(yc + 1 + 2, xc + 2)) / 4;
end