% Tato funkce slouzi k registraci dvou snimku pomoci kvadraticke transformace.
%
% Vstupni promenne:
%   souradnice_referencni     - prvni sloupec souradnice x    ... referenci
%                             - druhy sloupec souradnice y
%   souradnice_registrovany   - prvni sloupec souradnice x    ... registrovany
%                             - druhy sloupec souradnice y
%   snimek1      - referencni
%   snimek2      - registrovany


function [vysledny]=kvadraticka_transformace(souradnice_referencni, souradnice_registrovany, snimek2)
                   
            [nr,nc] = size( snimek2(:,:,1) );

            souradnice_referencni(:,1) = souradnice_referencni(:,1)/nc;
            souradnice_referencni(:,2) = souradnice_referencni(:,2)/nr;

            souradnice_registrovany(:,1) = souradnice_registrovany(:,1)/nc;
            souradnice_registrovany(:,2) = souradnice_registrovany(:,2)/nr;

            % Vypocet parametru kvadraticke transformace
            % pocet bodu
            [np,ll] = size( souradnice_registrovany );

            % Skladani matic pro reseni
            r1 = zeros(1,6); r2 = zeros(1,6); r3 = zeros(1,6); r4 = zeros(1,6); r5 = zeros(1,6); r6 = zeros(1,6);
            d1 = zeros(1,6);

            for ii = 1:np,

                r1 = r1 + [(souradnice_referencni(ii,1)^2), souradnice_referencni(ii,1)*souradnice_referencni(ii,2), (souradnice_referencni(ii,2)^2), souradnice_referencni(ii,1), souradnice_referencni(ii,2), 1]*souradnice_referencni(ii,1)^2;
                r2 = r2 + [(souradnice_referencni(ii,1)^2), souradnice_referencni(ii,1)*souradnice_referencni(ii,2), (souradnice_referencni(ii,2)^2), souradnice_referencni(ii,1), souradnice_referencni(ii,2), 1]*souradnice_referencni(ii,1)*souradnice_referencni(ii,2);
                r3 = r3 + [(souradnice_referencni(ii,1)^2), souradnice_referencni(ii,1)*souradnice_referencni(ii,2), (souradnice_referencni(ii,2)^2), souradnice_referencni(ii,1), souradnice_referencni(ii,2), 1]*souradnice_referencni(ii,2)^2;
                r4 = r4 + [(souradnice_referencni(ii,1)^2), souradnice_referencni(ii,1)*souradnice_referencni(ii,2), (souradnice_referencni(ii,2)^2), souradnice_referencni(ii,1), souradnice_referencni(ii,2), 1]*souradnice_referencni(ii,1);
                r5 = r5 + [(souradnice_referencni(ii,1)^2), souradnice_referencni(ii,1)*souradnice_referencni(ii,2), (souradnice_referencni(ii,2)^2), souradnice_referencni(ii,1), souradnice_referencni(ii,2), 1]*souradnice_referencni(ii,2);
                r6 = r6 + [(souradnice_referencni(ii,1)^2), souradnice_referencni(ii,1)*souradnice_referencni(ii,2), (souradnice_referencni(ii,2)^2), souradnice_referencni(ii,1), souradnice_referencni(ii,2), 1];

                d1 = d1 + souradnice_registrovany(ii,1)*[souradnice_referencni(ii,1)^2, souradnice_referencni(ii,1)*souradnice_referencni(ii,2), souradnice_referencni(ii,2)^2, souradnice_referencni(ii,1), souradnice_referencni(ii,2), 1];
            end
            M1 = [r1;r2;r3;r4;r5;r6];

            r1 = zeros(1,6); r2 = zeros(1,6); r3 = zeros(1,6); r4 = zeros(1,6); r5 = zeros(1,6); r6 = zeros(1,6);
            d2 = zeros(1,6);

            for ii = 1:np,

                r1 = r1 + [(souradnice_referencni(ii,1)^2), souradnice_referencni(ii,1)*souradnice_referencni(ii,2), (souradnice_referencni(ii,2)^2), souradnice_referencni(ii,1), souradnice_referencni(ii,2), 1]*souradnice_referencni(ii,1)^2;
                r2 = r2 + [(souradnice_referencni(ii,1)^2), souradnice_referencni(ii,1)*souradnice_referencni(ii,2), (souradnice_referencni(ii,2)^2), souradnice_referencni(ii,1), souradnice_referencni(ii,2), 1]*souradnice_referencni(ii,1)*souradnice_referencni(ii,2);
                r3 = r3 + [(souradnice_referencni(ii,1)^2), souradnice_referencni(ii,1)*souradnice_referencni(ii,2), (souradnice_referencni(ii,2)^2), souradnice_referencni(ii,1), souradnice_referencni(ii,2), 1]*souradnice_referencni(ii,2)^2;
                r4 = r4 + [(souradnice_referencni(ii,1)^2), souradnice_referencni(ii,1)*souradnice_referencni(ii,2), (souradnice_referencni(ii,2)^2), souradnice_referencni(ii,1), souradnice_referencni(ii,2), 1]*souradnice_referencni(ii,1);
                r5 = r5 + [(souradnice_referencni(ii,1)^2), souradnice_referencni(ii,1)*souradnice_referencni(ii,2), (souradnice_referencni(ii,2)^2), souradnice_referencni(ii,1), souradnice_referencni(ii,2), 1]*souradnice_referencni(ii,2);
                r6 = r6 + [(souradnice_referencni(ii,1)^2), souradnice_referencni(ii,1)*souradnice_referencni(ii,2), (souradnice_referencni(ii,2)^2), souradnice_referencni(ii,1), souradnice_referencni(ii,2), 1];

            %     d2 = d2 + [souradnice_registrovany(ii,2)*souradnice_referencni(ii,1)^2, souradnice_referencni(ii,2)*souradnice_referencni(ii,1)*souradnice_referencni(ii,2), souradnice_referencni(ii,2)*souradnice_referencni(ii,2)^2, souradnice_referencni(ii,2)*souradnice_referencni(ii,1), souradnice_referencni(ii,2)*souradnice_referencni(ii,2),souradnice_referencni(ii,2)];
                d2 = d2 + souradnice_registrovany(ii,2)*[souradnice_referencni(ii,1)^2, souradnice_referencni(ii,1)*souradnice_referencni(ii,2), souradnice_referencni(ii,2)^2, souradnice_referencni(ii,1), souradnice_referencni(ii,2), 1];
            end
            M2 = [r1;r2;r3;r4;r5;r6];

            M = [M1 zeros(size(M1));zeros(size(M1)) M2];
            d = [d1(:);d2(:)];

            % reseni rovnic
            % M = inv(M);
            x = M\d;

            % vysledna transformacni matice 
            M = [x(1:6)';x(7:12)'];

            % Tady, do Xnew a Ynew, ulozime kam se transformuji souradnice
            [X,Y]=meshgrid( linspace(0,1,nc), linspace(0,1,nr) );

            Xnew = zeros( nr, nc );
            Ynew = zeros( nr, nc );
            for r = 1:nr,
                 for c = 1:nc
                   Xnew(r,c) = M(1,1)*X(r,c)^2+M(1,2)*X(r,c)*Y(r,c)+M(1,3)*Y(r,c)^2+M(1,4)*X(r,c)+M(1,5)*Y(r,c)+M(1,6);
                   Ynew(r,c) = M(2,1)*X(r,c)^2+M(2,2)*X(r,c)*Y(r,c)+M(2,3)*Y(r,c)^2+M(2,4)*X(r,c)+M(2,5)*Y(r,c)+M(2,6);
                 end
            end

            % A interpoluje se
            for i=1:size(snimek2,3)
            pomocny= interp2( X, Y, double(snimek2(:,:,i)), Xnew, Ynew, 'linear', 0 );
            vysledny(:,:,i) =  uint8(pomocny(1:nr, 1:nc));
            end
            
end