function SOLS = solve36( L , T) 
% function SOLS = solve36( L , T) 
%
% L is the data input. 
% T is for debug use only. Take T from the output of the data generator. 
%
% SOLS is the set of possible solutions where columns are [a1;a2;b1;b2] 
%
% And we get rotation matrices for [ai;bi;-bi a1] 
% 
%
%clear all
%[ T,X,Pdummy,L] = solve36_generate_data( 3,6 );
%
%
% SOLS = solve36( L);
%
%   a1 = T{2}(1,1);
%   b1 = T{2}(1,2);
%   a2 = T{3}(1,1);
%   b2 = T{3}(1,2);
%SOLS-[a1;a2;b1;b2]*ones(1,size(SOLS,2))
%
%
%
% The code is as far as is ever possible from being optimized and 
% requires the symbolic toolbox. It shows however how a Grobner basis
% can be computed and the system then solved. 
%
%@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}
%}





EQ30 = buildEQ3( L , 1 );
EQ31 = buildEQ3( L , 2 );
EQ32 = buildEQ3( L , 3 );
EQ33 = buildEQ3( L , 4 );
EQ34 = buildEQ3( L , 5 );
EQ35 = buildEQ3( L , 6 );




if exist('T','var')    %%Kan vara bra för att kolla att man har fått tag på rätt ekvationer! 
   a1 = T{2}(1,1);
   b1 = T{2}(1,2);
   a2 = T{3}(1,1);
   b2 = T{3}(1,2);
   errvec = zeros(5,6);
   for i=1:5
      errvec(i,:)=eval( [  EQ30{i}  EQ31{i} EQ32{i}   EQ33{i}   EQ34{i}  EQ35{i}  ]);
   end
   disp(sum(abs(errvec(:))))
   EQ2 = [ a1^2+b1^2-1,a2^2+b2^2-1 ] ; 
end



EQ = {EQ30{:}, EQ31{:}, EQ32{:}};
%%Här blir det lurigt. Nu skall jag räkna rester! 

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

A = [];
MONS=[1 1 0 0 1 1 0 0 1 1 0 0 1 1 0 0 1 1 0 0 1 0 0 0 1 1 0 0 1 1 0 0 1 1 0 0 1 1 0 0 1 0 0 0 1 1 0 0 1 1 0 0 1 1 0 0 1 0 0 0 1 1 0 0 1 1 0 0 1 0 0 0 1 1 0 0 1 0 0 0 1 0 0 0 0;1 0 1 0 1 0 1 0 1 0 1 0 1 0 1 0 1 0 1 0 0 1 0 0 1 0 1 0 1 0 1 0 1 0 1 0 1 0 1 0 0 1 0 0 1 0 1 0 1 0 1 0 1 0 1 0 0 1 0 0 1 0 1 0 1 0 1 0 0 1 0 0 1 0 1 0 0 1 0 0 0 1 0 0 0;4 5 5 6 3 4 4 5 2 3 3 4 1 2 2 3 0 1 1 2 0 0 1 0 3 4 4 5 2 3 3 4 1 2 2 3 0 1 1 2 0 0 1 0 2 3 3 4 1 2 2 3 0 1 1 2 0 0 1 0 1 2 2 3 0 1 1 2 0 0 1 0 0 1 1 2 0 0 1 0 0 0 1 0 0;0 0 0 0 1 1 1 1 2 2 2 2 3 3 3 3 4 4 4 4 5 5 5 6 0 0 0 0 1 1 1 1 2 2 2 2 3 3 3 3 4 4 4 5 0 0 0 0 1 1 1 1 2 2 2 2 3 3 3 4 0 0 0 0 1 1 1 1 2 2 2 3 0 0 0 0 1 1 1 2 0 0 0 1 0]; 

for i=1:15   
   EQ{i}= maple('algsubs' ,  'a1^2=1-b1^2', EQ{i}  ); 
   EQ{i}= maple('expand' ,  EQ{i}  ); 
   EQ{i}= maple('algsubs' ,  'a2^2=1-b2^2', EQ{i}  ); 
   EQ{i}= maple('expand' ,  EQ{i}  ); 
   for j=1:size(MONS,2)
      A(i,j) =eval( mcoeff2(   EQ{i} , MONS(:,j)));
   end 
end


if exist('T','var')    %%Kan vara bra för att kolla att man har fått tag på rätt ekvationer! 
  %% Dessa residualer skall vara VÄLDIGT små! 
  mons= (a1.^MONS(1,:).*a2.^MONS(2,:).*b1.^MONS(3,:).*b2.^MONS(4,:))' ;
  disp( norm(A*mons))
end 



%%Matrisen A har rank 6 
if ~(rank(A)==6), error('rank!=6'),end 

I=( sum(abs( A~= 0) ) )~=0;

[L,U] = lu(A(:,I));


for i=1:6
   U(i,:) = U(i,:) / U(i,i);
end


Iselect_mons=( sum(abs( A~= 0) ) )~=0;
B = A(:,Iselect_mons);
col=0;
for i=1:6
   col = col+1;
   switch col
    case 3 
       col=5;
    case 7
     col=13;
    otherwise
   end
   [dummy, I] = sort( abs( B(i:end,col)));
   B= B([1:(i-1) I(end:-1:1)'+(i-1) ] ,:) ; 
   B(i,:) = B(i,:) / B(i,col); 
   for j=1:size(B,1)
      if i~=j
	 B(j,:) = B(j,:)- B(j,col)*B(i,:);
      end
   end
 %  B*mons( Iselect_mons)
end

B = B(1:6,:);
B(5:6,1:12) = 0;
B( [1 3 4 5 ],15) = 0;
B( [2 3 4 6 ],16) = 0;
C= zeros(6,85);
C(:,Iselect_mons) = B;


%%Nu är gradtalen 6 6 5 5 4 4 
%% De som är grad 6 går vidare omultiplicerade övriga multipliceras
%% med allt som de tål! 

if 0 
D =[ C* mult(  [0;0;0;0] ); 
     C(3:end,:)* mult(  [1;0;0;0] ); 
     C(3:end,:)* mult(  [0;1;0;0] ); 
     C(3:end,:)* mult(  [0;0;1;0] ); 
     C(3:end,:)* mult(  [0;0;0;1] ); 
     C(6:end,:)* mult(  [2;0;0;0] );
     C(6:end,:)* mult(  [1;1;0;0] );
     C(6:end,:)* mult(  [1;0;1;0] );
     C(6:end,:)* mult(  [1;0;0;1] );
     C(6:end,:)* mult(  [0;2;0;0] );
     C(6:end,:)* mult(  [0;1;1;0] );
     C(6:end,:)* mult(  [0;1;0;1] );
     C(6:end,:)* mult(  [0;0;2;0] );
     C(6:end,:)* mult(  [0;0;1;1] );
     C(6:end,:)* mult(  [0;0;0;2] );  ];
else
  load solve36_multmats.mat
D =[ C* mult0000; 
     C(3:end,:)* mult1000; 
     C(3:end,:)* mult0100; 
     C(3:end,:)* mult0010; 
     C(3:end,:)* mult0001; 
     C(6:end,:)* mult2000;
     C(6:end,:)* mult1100;
     C(6:end,:)* mult1010;
     C(6:end,:)* mult1001;
     C(6:end,:)* mult0200;
     C(6:end,:)* mult0110;
     C(6:end,:)* mult0101;
     C(6:end,:)* mult0020;
     C(6:end,:)* mult0011;
     C(6:end,:)* mult0002;  ];
end 

if exist('T','var')    %%Kan vara bra för att kolla att man har fått tag på rätt ekvationer! 
  disp( norm( D*mons))
end 


E=D;
col=0;
for i=1:30
   col = col+1;
   while max( abs( E(i:end,col))) < 1e-10
      col= col+1;
   end
   [dummy, I] = sort( abs( E(i:end,col)));
   E= E([1:(i-1) I(end:-1:1)'+(i-1) ] ,:) ; 
   E(i,:) = E(i,:) / E(i,col); 
   for j=1:size(E,1)
      if i~=j
	 E(j,:) = E(j,:)- E(j,col)*E(i,:);
      end
   end
   end
E = E(1:30,:);
if exist('T','var')    %%Kan vara bra för att kolla att man har fått tag på rätt ekvationer! 
  disp( norm(E*mons) ) 
end 
%% 10*6  10*5 10*4 

I6 = 1:30;
I5 = 11:30;
I4 = 21:30;

if 0 
F =[ E(I6,:)* mult(  [0;0;0;0] ); 
     E(I5,:)* mult(  [1;0;0;0] ); 
     E(I5,:)* mult(  [0;1;0;0] ); 
     E(I5,:)* mult(  [0;0;1;0] ); 
     E(I4,:)* mult(  [2;0;0;0] );
     E(I4,:)* mult(  [1;1;0;0] );
     E(I4,:)* mult(  [1;0;1;0] );
     E(I4,:)* mult(  [1;0;0;1] );
     E(I4,:)* mult(  [0;2;0;0] );
     E(I4,:)* mult(  [0;1;1;0] );
     E(I4,:)* mult(  [0;1;0;1] );
     E(I4,:)* mult(  [0;0;2;0] );
     E(I4,:)* mult(  [0;0;1;1] );
     E(I4,:)* mult(  [0;0;0;2] );  ];
else
F =[ E(I6,:)* mult0000; 
     E(I5,:)* mult1000; 
     E(I5,:)* mult0100; 
     E(I5,:)* mult0010; 
     E(I4,:)* mult2000;
     E(I4,:)* mult1100;
     E(I4,:)* mult1010;
     E(I4,:)* mult1001;
     E(I4,:)* mult0200;
     E(I4,:)* mult0110;
     E(I4,:)* mult0101;
     E(I4,:)* mult0020;
     E(I4,:)* mult0011;
     E(I4,:)* mult0002;  ];
end 
if exist('T','var')    %%Kan vara bra för att kolla att man har fått tag på rätt ekvationer! 
  disp( norm( F*mons) ) 
end 

G= F;

col=0;
for i=1:rank(F)
   col = col+1;
   while max( abs( G(i:end,col))) < 1e-10
      col= col+1;
   end
   [dummy, I] = sort( abs( G(i:end,col)));
   G= G([1:(i-1) I(end:-1:1)'+(i-1) ] ,:) ; 
   G(i,:) = G(i,:) / G(i,col); 
   for j=1:size(G,1)
      if i~=j
	 G(j,:) = G(j,:)- G(j,col)*G(i,:);
      end
   end
end
if exist('T','var')    %%Kan vara bra för att kolla att man har fått tag på rätt ekvationer! 
  disp(norm(G*mons))
end 
G = G( 1:59,:) ;

if 0 
  M=  [zeros( 26,59) eye(26)]* mult(  [1;0;0;0] );
else
  M=  [zeros( 26,59) eye(26)]* mult1000;
end 
for row=1:26
   for col=1:59
      M(row,: )=    M(row,: )- M(row,col )*G(col,:);
   end
end



MM= M( :, 60:end) ;
[V,D] = eig( MM); 
V = V./(ones(size(V,1),1)*V(end,:)); 

SOLS =  V((end-4):(end-1),:); 


if exist('T','var')    %%Kan vara bra för att kolla att man har fått tag på rätt ekvationer! 
  err = min( sum( (SOLS-[a1;a2;b1;b2]*ones(1,size(SOLS,2))).^2).^0.5)
  disp(err)
end 



function EQ3 = buildEQ3( L , n ) 

selQ= inline( '[ -L{i}(n,3) / L{i}(n,1) ;0;] ', 'L', 'n', 'i');
selQp= inline( '[ L{i}(n,2) ; -L{i}(n,1) ; ] ', 'L', 'n', 'i');
 
syms l1 l2 l3  
syms a1 a2 b1 b2 	

%%Nu skall jag plocka ut en (godtycklig) punkt och riktning
%% på stråle nummer n. 


Q0  = selQ(L,n,1);
Q0p =  selQp(L,n,1);
Q1  =  selQ(L,n,2);
Q1p =  selQp(L,n,2);
Q2  =  selQ(L,n,3);
Q2p =  selQp(L,n,3);

t1 = Q0 + l1 * Q0p;
t2 = Q1 + l2 * Q1p;
t3 = Q2 + l3 * Q2p;
               
R1 = eye(2);
R2  = [a1 b1; -b1 a1 ];
R3  = [a2 b2; -b2 a2 ];

N = [0 0 1 ];
     
T1 = [R1  t1 ; N ];
T2 = [R2  t2 ; N ];
T3 = [R3  t3 ; N ];

for i=1:6     
   EQ1{i}= det( [ L{1}(i,:) *T1 ;  L{2}(i,:) *T2;  L{3}(i,:) *T3  ] ) ;
end

coeff= inline('maple(''coeff'',f,x,n)','f','x','n');
mcoeff= inline('maple(''coeff'',maple(''coeff'',maple(''coeff'',f,''l1'',n1),''l2'',n2),''l3'',n3)','f','n1','n2','n3');

K= zeros(6,4)*a1;

for i=1:6
   t = EQ1{i};
   K(i,:) = [ mcoeff(t,0,0,0) mcoeff(t,1,0,0) mcoeff(t,0,1,0)  mcoeff(t,0,0,1)];
end
%%Nu är rad n i K ointressant då den är NOLL. 
K= K([1:(n-1) (n+1):end ], :); 
subsets=[[1;2;3;4],[1;2;3;5],[1;2;4;5],[1;3;4;5],[2;3;4;5]];
for i=1:5
   EQ3{i}= det( K(subsets(:,i),:));
end







function B = mult( monom )
MONS = build_mons( 6 ) ;
eq1=  multipol( [1 1 -1 ] , [ 2 0 0 ; 0 0 0 ;  0 2 0 ; 0 0 0  ]);
eq2=  multipol( [1 1 -1 ] , [ 0 0 0 ; 2 0 0 ;  0 0 0 ; 0 2 0  ]);
NOLLPOLYNOMET = multipol( zeros(1,85), MONS ) ;
B = [ ];
%%Jag tror att jag skall g%G�%@a det med min multipol-klass.
for p = MONS
   if sum(p+monom) > 6
      B = [B; zeros(1,85) ];
   else
      p = multipol( 1, p+monom(:));
      p = rem(p, eq1);
      p = rem(p, eq2);
      %%Nu m%G�%@te vi handskas med att pnew(1) eller pnew(2) >=2
      p = cleanup( p + NOLLPOLYNOMET );
      p_mon = coeffs( p );
      B = [B; p_mon ];
   end
end


function MONS = build_mons( n )
MONS =[ ]  ;
for a1= 0:1
   for a2= 0:1
      for b1= 0:n
         for b2= 0:n
            if a1+a2+b1+b2 <= n
              MONS =[MONS [ a1;a2;b1;b2]]  ;
            end
         end
      end
   end
end
[dummy,I]=sortrows(  [sum(MONS);  -MONS(end:-1:1,:) ]' ) ;
MONS = MONS(:,I(end:-1:1));