function [SOLS ] = sm32grevlex( l ) 
%function [SOLS ] = sm32grevlex( l ) 
%
% l is a 2x2x3 cell array containing planes defining the 
% problem. See paper example code or paper cited below. 
%
% SOLS has as columns [a1;b1;a2;b2] 
% which gives the rotation matrices at times 2 and 3. 
% [ai bi;-bi ai]
%
%
% This function depends on the files 
%    solve_32grevlex_K.mat
%    solve_32grevlex_multmats.mat
% being visible. 
%
% The code also depends on MAPLE being available. 
%  
%EXAMPLE AND TEST CODE. THIS SHOULD WORK AND GIVE 0 RESIDUALS!
% clear all
%[T,X,P] = planar_mov_randconf( 3,2 );
%
%for k=1:2
%   for i=1:3
%      temp = null( [(T{i}*X{k})' ; null(P{k})' ] )';
%      for j=1:2
%         l{j,k,i} = temp(j,:);
%      end
%   end
%end
%
%a1 = T{2}(1,1);
%b1 = T{2}(1,2);
%a2 = T{3}(1,1);
%b2 = T{3}(1,2);
%
%   SOLS = solve_32grevlex( l ) ;
%
%disp([(SOLS(1,:).^2 + SOLS(3,:).^2 -1); 
%      (SOLS(2,:).^2 + SOLS(4,:).^2 -1)])
%
%terr = SOLS - ([a1;a2;b1;b2]*ones(1,4));
%disp( min(sum(abs(terr))))
%
%
%
%
%
% Read more in: 
%
%@PHDTHESIS{stewenius-pdh-2005,
%  AUTHOR = {H. Stew\'enius},
%  TITLE = {Gr{\"o}bner Basis Methods for Minimal Problems in Computer Vision},
%  SCHOOL = {Lund University},
%  YEAR = 2005,
%  MONTH = APR,
%  PDF = {http://www.maths.lth.se/matematiklth/personal/stewe/THESIS/stewenius_phd_2005.pdf}
%}
%
%
%
%@INPROCEEDINGS{stewenius-astrom-eccv-2004,
%  AUTHOR = {H. Stew\'enius and K. {\AA}str{\"o}m},
%  TITLE = {Structure and Motion Problems for Multiple Rigidly Moving Cameras},
%  BOOKTITLE = {European Conference on Computer Vision (ECCV)},
%  YEAR = 2004,
%  MONTH = MAY,
%  ADDRESS = {Prague , Czech Republic},
%  PAGES = {238ff},
%  NOTE = {An improved version of this in Chapter 9 of my thesis}
%}




[M, eq10] = buildMgrevlex( l) ;

for i=1:12
   M(i,:) = M(i,:) - M(i,1)*M(13,:) - M(i,3)*M(14,:);
end


if 1 
   M2 = M(13:14,:);
   M = M(1:12,[2 5 11] ) \M(1:12,:)  ;
   M= [M2(1,:);
       M(1,:);
       M2(2,:);
       M(2:3,:) ];
   M([2 5],7) = 0; 
   M(4:5,9) = 0;
else   
   %%Detta svarar mot Gauss-Jordan
   [U,S,V] = svd( M(1:12,:) ) ;
   M = V(:,1:3)' ;

   M = inv(M(:,[ 2  5 11 ] ))*M;
end

M(:,[1 2 3 5 11 ] ) = eye( 5) ;



%% Sist i M sitter en ekvation som är linjär i (a1,a2,b1,b2,1)
%% Denna skall multipliceras upp och gj köras igen. 


if 0
   build_multmats
else
   load solve_32grevlex_multmats.mat
end



t = M(5,:);
M = [M ; t*M22a1 ; t*M22a2 ; t*M22b1 ; t*M22b2 ];

M = inv( M(:,[1:8  11] ) ) *M;

M(:,[1:8  11] ) = eye(9);

%Skall nu slänga allt som har att göra med a1
I = [3  5  6  8  9 10 12 13 14 15];
M = M([3 5 6 8],I) ;
 


Mnew = [M*M23ett; M*M23a2;M*M23b1; M*M23b2 ];


if 0 
   %%Detta svarar mot Gauss-Jordan
   [U,S,V] = svd( Mnew ) ;
   Mnew = V(:,1:14)' ;
   Mnew = inv(  Mnew(:,[1 2 3 4 5 6 7  10 11 12 13 14 15 16 ]  ))*Mnew ;
   Mnew(:,[1 2 3 4 5 6 7  10 11 12 13 14 15 16 ]  ) = eye( 14 );
else
   tt = Mnew([9 5 6 7 13 14 15 16 1 2 3 4  8  10 11 12 ], : )  ;
   tt = [tt(1:12,:) 
	 tt(14,:)-tt(2,:)
	 tt(13,:)-tt(6,:)
	 tt(15,:)-tt(3,:)
	 tt(16,:)-tt(5,:)];
   for i=[13 15 16]
      tt(i,:) = tt(i,:) - tt(i,11)*tt(9,:);
   end
   
   
   Mnew = tt(:,[1 2 3 4 5 6 7  10 11 12 13 14 15 16 ])\tt;
   Mnew(:,[1 2 3 4 5 6 7  10 11 12 13 14 15 16 ]  ) = eye( 14 );
   Mnew(9:end,[8 9] ) = 0;
   Mnew(1,:)=  [1 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 -1 0 0 0];
   Mnew(2,:) =[0 1 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 -1 0 0];
end


   
%%
%%Nu tror jag att jag har en GB. 
%%Antalet inre element ar ratt. 



N = [-Mnew(end-2:end,end-3:end) ; 0 0 1 0 ];

[V,D] = eig( N );

V = V./(ones(4,1)*V(end,:));

% V(1,:).^2+V(3,:).^2 - 1 

clear i
for cnt=1:4
   syms a1 
   a2 = V(1,cnt);
   b1 = V(2,cnt);
   b2 = V(3,cnt);
   
   a1 = eval(maple('fsolve', eval(eq10)  ));
 %  a1^2+b1^2-1    
   SOLS(:,cnt) = [a1;a2;b1;b2];
end




function [M, eq10] = buildMgrevlex( l ) 
K = insertK(l );

for i=1:12
   I = [1:(i-1) (i+1):12];
   eq3{i} = det( K(I,:)) ;
end


% a_1^2 a_1a_2 a_2^2 a_2b_1 b_1^2 a_1b_2 b_1b_2 b_2^2 a_1 a_2 b_1 b_2 1 
MONS=[
   2 1 0 1 0 0 1 0 0 0 1 0 0 0 0;
   0 1 2 0 1 0 0 1 0 0 0 1 0 0 0;
      0 0 0 1 1 2 0 0 1 0 0 0 1 0 0; 
   0 0 0 0 0 0 1 1 1 2 0 0 0 1 0; ];



mcoeff=inline(['maple(''coeff'', '...
	       'maple(''coeff'', '...
	       'maple(''coeff'', '...
	       'maple(''coeff'','...
	       'f,''a1'',n(1)),''a2'',n(2)),''b1'',n(3)),''b2'',n(4))'] ...
	      ,'f','n');

syms a1 a2 b1 b2 
eq3{13} = a1^2+b1^2-1;
eq3{14} = a2^2+b2^2-1;
clear M 
for i=1:14
   for j=1:15
      M(i,j) = eval( mcoeff( eq3{i}, MONS(:,j) ));
   end
end

eq10 = eq3{10};




function K = insertK( l ) ;

syms a1 a2 b1 b2

load solve_32grevlex_K.mat
for k=1:2
   for j=1:2
      for i=1:3
         %l{k,j,i} = randn(1,4);
         for ii =1:4;
            varname =  sprintf('l%i%i%i_%i',k,j,i,ii);
            value = l{k,j,i}(ii);
            %disp([varname-value])
            eval(sprintf( '%s =  l{k,j,i}(ii);', varname ));
         end
      end
   end
end
K = eval( K) ;
clear l1* l2* l3*