% TP Codages JPEG et MPEG-2 - 3SN-M - 2022

%--------------------------------------------------------------------------
% Fonction d'estimation du mouvement par "block-matching"
%--------------------------------------------------------------------------
% MVr = EstimationMouvement(Ic,Ir)
%
% sortie  : MVdr = matrice des vecteurs de deplacements relatifs
% 
% entrees : Ic = image courante
%           Ir = image de reference
%--------------------------------------------------------------------------

function MVr = EstimationMouvement(Ic, Ir)
    
    fun = @(bloc) CSAMacroBloc(bloc.data, bloc.location, Ir);
    MVr = blockproc(Ic, [16, 16], fun);

end


%--------------------------------------------------------------------------
% Fonction de recherche par 'Cross Search Algorithm' :         
%   - Recherche pour un macro-bloc de l'image courante
%--------------------------------------------------------------------------
% [MB_IP,Vm] = CSAMacroBloc(MBc, Vp, Iref)
%
% sorties : MB_IP = macro-bloc de prediction dans Iref
%           Vm = vecteur de mouvement 
% 
% entrées : Mbc = macro-bloc dans l'image courante Ic
%           Vp = vecteur de prediction (point de depart du MacroBloc)
%           Iref = image de reference (qui sera conservee dans le GOP)
%--------------------------------------------------------------------------

function Vm = CSAMacroBloc(MB_IC, Vp, IRef)

    %-------------------------------------------------------------------------%
    % Initialisation                                                          %
    %-------------------------------------------------------------------------%

    Vm = zeros(1, 1, 2);
    coordx = Vp(1):Vp(1)+15;
    coordy = Vp(2):Vp(2)+15;
    
    %-------------------------------------------------------------------------%
    % Vérification du minimum des 9 EQM et déplacement ou arrêt               %
    % en fonction de la position du minimum parmi les 9                       %
    %-------------------------------------------------------------------------%

    while true

%         directions = [
%             0, 0
%             -1, 0
%             0, -1
%             1, 0
%             0, 1
%         ]; % 5 directions
        directions = fullfact([3, 3]) - 2; % 9 directions

        fun = @(direction) EQMMacrocBlocVoisin(MB_IC, IRef, size(IRef, 1), size(IRef, 2), coordx, coordy, direction.data);
        EQMs = blockproc(directions, [1, 2], fun);

        [~, index] = min(EQMs);
        direction_min = directions(index, :);
    
        if isequal(direction_min, [0, 0])
            break
        end

        coordx = coordx + direction_min(1);
        coordy = coordy + direction_min(2);
        Vm(1, 1, 1) = Vm(1, 1, 1) + direction_min(1);
        Vm(1, 1, 2) = Vm(1, 1, 2) + direction_min(2);
 
    end

end

%--------------------------------------------------------------------------
% Fonction de calcul de l'EQM avec differents voisins 
% dans l'image de reference
%--------------------------------------------------------------------------
% EQM = EQMMacrocBlocVoisin(MB_IC_V,IRef,size_x_Ref,size_y_Ref,coordx,coordy,voisin)
%
% sortie  : EQM = erreur quadratique moyenne entre macro-blocs
% 
% entrées : MB_IC_V = macro-bloc dans l'image courante (vectorise)
%           Ir = Image de reference
%           size_x_Ir = nombre de lignes de Ir (pour effets de bords)
%           size_y_Ir = nombre de colonnes de Ir (pour effets de bords)
%           coordx = les 16 coordonnees du bloc suivant x
%           coordy = les 16 coordonnees du bloc suivant y
%           voisin = choix du voisin pour decaler le macro-bloc dans Ir
%                    ('haut', 'gauche', 'centre', 'droite', bas', ...)
%--------------------------------------------------------------------------


function EQM = EQMMacrocBlocVoisin(MB_IC_V, Ir, size_x_Ir, size_y_Ir, coordx, coordy, voisin)

    coordx = coordx + voisin(1);
    coordy = coordy + voisin(2);

    if sum((coordy < 1) + (coordx < 1) + (coordy > size_y_Ir) + (coordx > size_x_Ir)) > 0
        EQM = +inf;
    else
        patch = Ir(coordx, coordy);
        EQM = mean((MB_IC_V(:) - patch(:)).^2);
    end

end