% p_fixed   - prvni sloupec souradnice x    ... referenci
%           - druhy sloupec souradnice y
% p_move    - prvni sloupec souradnice x    ... registrovany
%           - druhy sloupec souradnice y
% rgb2      - barevny referencni
% rgb1      - barevny registrovany

function [barevny, sachovnice]=kvadraticka_transformace(p_fixed, p_move, rgb2, rgb1)
                   
            [nr,nc] = size( rgb1(:,:,2) );

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

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

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

            % Skladani matic pro reseni
            A = zeros(12*np,12);
            i = 1;
            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 + [(p_fixed(ii,1)^2), p_fixed(ii,1)*p_fixed(ii,2), (p_fixed(ii,2)^2), p_fixed(ii,1), p_fixed(ii,2), 1]*p_fixed(ii,1)^2;
                r2 = r2 + [(p_fixed(ii,1)^2), p_fixed(ii,1)*p_fixed(ii,2), (p_fixed(ii,2)^2), p_fixed(ii,1), p_fixed(ii,2), 1]*p_fixed(ii,1)*p_fixed(ii,2);
                r3 = r3 + [(p_fixed(ii,1)^2), p_fixed(ii,1)*p_fixed(ii,2), (p_fixed(ii,2)^2), p_fixed(ii,1), p_fixed(ii,2), 1]*p_fixed(ii,2)^2;
                r4 = r4 + [(p_fixed(ii,1)^2), p_fixed(ii,1)*p_fixed(ii,2), (p_fixed(ii,2)^2), p_fixed(ii,1), p_fixed(ii,2), 1]*p_fixed(ii,1);
                r5 = r5 + [(p_fixed(ii,1)^2), p_fixed(ii,1)*p_fixed(ii,2), (p_fixed(ii,2)^2), p_fixed(ii,1), p_fixed(ii,2), 1]*p_fixed(ii,2);
                r6 = r6 + [(p_fixed(ii,1)^2), p_fixed(ii,1)*p_fixed(ii,2), (p_fixed(ii,2)^2), p_fixed(ii,1), p_fixed(ii,2), 1];

                d1 = d1 + p_move(ii,1)*[p_fixed(ii,1)^2, p_fixed(ii,1)*p_fixed(ii,2), p_fixed(ii,2)^2, p_fixed(ii,1), p_fixed(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 + [(p_fixed(ii,1)^2), p_fixed(ii,1)*p_fixed(ii,2), (p_fixed(ii,2)^2), p_fixed(ii,1), p_fixed(ii,2), 1]*p_fixed(ii,1)^2;
                r2 = r2 + [(p_fixed(ii,1)^2), p_fixed(ii,1)*p_fixed(ii,2), (p_fixed(ii,2)^2), p_fixed(ii,1), p_fixed(ii,2), 1]*p_fixed(ii,1)*p_fixed(ii,2);
                r3 = r3 + [(p_fixed(ii,1)^2), p_fixed(ii,1)*p_fixed(ii,2), (p_fixed(ii,2)^2), p_fixed(ii,1), p_fixed(ii,2), 1]*p_fixed(ii,2)^2;
                r4 = r4 + [(p_fixed(ii,1)^2), p_fixed(ii,1)*p_fixed(ii,2), (p_fixed(ii,2)^2), p_fixed(ii,1), p_fixed(ii,2), 1]*p_fixed(ii,1);
                r5 = r5 + [(p_fixed(ii,1)^2), p_fixed(ii,1)*p_fixed(ii,2), (p_fixed(ii,2)^2), p_fixed(ii,1), p_fixed(ii,2), 1]*p_fixed(ii,2);
                r6 = r6 + [(p_fixed(ii,1)^2), p_fixed(ii,1)*p_fixed(ii,2), (p_fixed(ii,2)^2), p_fixed(ii,1), p_fixed(ii,2), 1];

            %     d2 = d2 + [p_move(ii,2)*p_fixed(ii,1)^2, p_fixed(ii,2)*p_fixed(ii,1)*p_fixed(ii,2), p_fixed(ii,2)*p_fixed(ii,2)^2, p_fixed(ii,2)*p_fixed(ii,1), p_fixed(ii,2)*p_fixed(ii,2),p_fixed(ii,2)];
                d2 = d2 + p_move(ii,2)*[p_fixed(ii,1)^2, p_fixed(ii,1)*p_fixed(ii,2), p_fixed(ii,2)^2, p_fixed(ii,1), p_fixed(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;
            Mout = M;

            % 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
            pomocny= interp2( X, Y, double(rgb1(:,:,1)), Xnew, Ynew, 'linear', 0 );
            barevny(:,:,1) =  uint8(pomocny(1:nr, 1:nc));
            
            pomocny= interp2( X, Y, double(rgb1(:,:,2)), Xnew, Ynew, 'linear', 0 );
            barevny(:,:,2) =  uint8(pomocny(1:nr, 1:nc));
            
            pomocny= interp2( X, Y, double(rgb1(:,:,3)), Xnew, Ynew, 'linear', 0 );
            barevny(:,:,3) =  uint8(pomocny(1:nr, 1:nc));
          

            %% sachovnice
            sachovnice = MakeChessBoard( iluminace(rgb2(:,:,2)), iluminace(barevny(:,:,2)), 150 );   