function SOLS = solve62lineversion( L)
%function SOLS = solve62lineversion( L)
%
% If we have a multicamera rig and all cameras are on 
% a single line on the vehicle. Then the normal solver is degenerate 
% and this specially crafted solver must be used instead. It is 
% It is important to note that for this solver there are several 
% non-trivial degenereate cases, most interesting of these (as with 
% all multicam) is pure translation. Note that if the rotation is along the
% axis on which the cameras are mounted this is also a degenerate case. 
%
% As with the standard version the first argument is a cell matrix 
% consisting of two 6x6 matrices with rays represented in plucker coordinates. 
% The answer comes as quaternions. 
%
% Tests have only been performed when component 4 of the 
% plucker coordinates is set to zero. It is possible that it works for other 
% linear degeneracies but currently untested. 
%
%
% This code is in a state VERY FAR from being optimized and was written
% just to provide a proof of concept. 
%
%
%
%
%
%Please refer to:
%
%
%@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}
%}
%
%
% If you use this program



if( not((exist('build_eq2')==3) && (exist('first_row')==3) && (exist('initM')==3) && (exist('last_gb')==3) && (exist('mult_w_all')==3) ))
    fprintf(['On or more of the neded mexfiles are unavailable.\nPlease compile all needed files and then try again\nSuggested way to do this in matlab (on Linux) is:\n   mex -DMAIN_BUILD_EQ2 solve26.c -output  build_eq2\n']); 

if( not(exist('solve26.c')))
    error('File solve26.c is missing. You will not be able to compile');
end

if(not(exist('solve26AFILE.c')  ))
    error('File solve26AFILE.c is missing. You will not be able to compile');
end
    error('Unable to Run');
end






%% Column 1 är död  
A(:,[  1    29    50    65    75    81    84]) = 0;


B = A(:,2:16)\A ;

B(:,2:16) = eye(15);

%% Nu skall vi mätta. 

f1= partSatN( B, 1 ) ;
f2=partSatN( B, 2 ) ;
f3=partSatN( B, 3 ) ;

AA = [B ; f1;f2;f3];

%%Denna har rank 28 (som väntat) 


MM = AA(:,1:28)\AA;

NN = zeros(56);
inner_points =[29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 ];
NN([1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 ],: ) = -MM([1 2 3 4 5 6 8 9 10 11 12 14 15 16 17 19 20 21 23 24 26 ],inner_points);
NN([22 79 136 193 250 363 420 477 534 647 704 761 874 931 1044 1213 1270 1327 1384 1497 1554 1611 1724 1781 1894 2063 2120 2177 2290 2347 2460 2629 2686 2799 2968 ]) = 1;

[V,D] = eig(NN);

SOLS = V(end-3:end-1,:) ./(ones(3,1)*V(end,:));

% MONS = build_MONS(6,3);
MONS=[6 5 4 3 2 1 0 5 4 3 2 1 0 4 3 2 1 0 3 2 1 0 2 1 0 1 0 0 5 4 3 2 1 0 4 3 2 1 0 3 2 1 0 2 1 0 1 0 0 4 3 2 1 0 3 2 1 0 2 1 0 1 0 0 3 2 1 0 2 1 0 1 0 0 2 1 0 1 0 0 1 0 0 0;
	0 1 2 3 4 5 6 0 1 2 3 4 5 0 1 2 3 4 0 1 2 3 0 1 2 0 1 0 0 1 2 3 4 5 0 1 2 3 4 0 1 2 3 0 1 2 0 1 0 0 1 2 3 4 0 1 2 3 0 1 2 0 1 0 0 1 2 3 0 1 2 0 1 0 0 1 2 0 1 0 0 1 0 0;
	0 0 0 0 0 0 0 1 1 1 1 1 1 2 2 2 2 2 3 3 3 3 4 4 4 5 5 6 0 0 0 0 0 0 1 1 1 1 1 2 2 2 2 3 3 3 4 4 5 0 0 0 0 0 1 1 1 1 2 2 2 3 3 4 0 0 0 0 1 1 1 2 2 3 0 0 0 1 1 2 0 0 1 0;];






%SOLSout = [];
%for i=1:54
%   if abs( imag( SOLS(1,i)) ) < 1e-4
%      q2dcm( [SOLS(:,i); 1] )
%      SOLSout = [SOLSout SOLS(:,i)];
%   end   
%end




function newM = partSatN( M , k ) 

%addpath ../common_tools/
%MONS6 = build_MONS( 6, 3  );
%MONS7 = build_MONS( 7, 3  );

MONS6 =[6 5 4 3 2 1 0 5 4 3 2 1 0 4 3 2 1 0 3 2 1 0 2 1 0 1 0 0 5 4 3 2 1 0 4 3 2 1 0 3 2 1 0 2 1 0 1 0 0 4 3 2 1 0 3 2 1 0 2 1 0 1 0 0 3 2 1 0 2 1 0 1 0 0 2 1 0 1 0 0 1 0 0 0;0 1 2 3 4 5 6 0 1 2 3 4 5 0 1 2 3 4 0 1 2 3 0 1 2 0 1 0 0 1 2 3 4 5 0 1 2 3 4 0 1 2 3 0 1 2 0 1 0 0 1 2 3 4 0 1 2 3 0 1 2 0 1 0 0 1 2 3 0 1 2 0 1 0 0 1 2 0 1 0 0 1 0 0;0 0 0 0 0 0 0 1 1 1 1 1 1 2 2 2 2 2 3 3 3 3 4 4 4 5 5 6 0 0 0 0 0 0 1 1 1 1 1 2 2 2 2 3 3 3 4 4 5 0 0 0 0 0 1 1 1 1 2 2 2 3 3 4 0 0 0 0 1 1 1 2 2 3 0 0 0 1 1 2 0 0 1 0]; 
MONS7=[7 6 5 4 3 2 1 0 6 5 4 3 2 1 0 5 4 3 2 1 0 4 3 2 1 0 3 2 1 0 2 1 0 1 0 0 6 5 4 3 2 1 0 5 4 3 2 1 0 4 3 2 1 0 3 2 1 0 2 1 0 1 0 0 5 4 3 2 1 0 4 3 2 1 0 3 2 1 0 2 1 0 1 0 0 4 3 2 1 0 3 2 1 0 2 1 0 1 0 0 3 2 1 0 2 1 0 1 0 0 2 1 0 1 0 0 1 0 0 0;0 1 2 3 4 5 6 7 0 1 2 3 4 5 6 0 1 2 3 4 5 0 1 2 3 4 0 1 2 3 0 1 2 0 1 0 0 1 2 3 4 5 6 0 1 2 3 4 5 0 1 2 3 4 0 1 2 3 0 1 2 0 1 0 0 1 2 3 4 5 0 1 2 3 4 0 1 2 3 0 1 2 0 1 0 0 1 2 3 4 0 1 2 3 0 1 2 0 1 0 0 1 2 3 0 1 2 0 1 0 0 1 2 0 1 0 0 1 0 0;0 0 0 0 0 0 0 0 1 1 1 1 1 1 1 2 2 2 2 2 2 3 3 3 3 3 4 4 4 4 5 5 5 6 6 7 0 0 0 0 0 0 0 1 1 1 1 1 1 2 2 2 2 2 3 3 3 3 4 4 4 5 5 6 0 0 0 0 0 0 1 1 1 1 1 2 2 2 2 3 3 3 4 4 5 0 0 0 0 0 1 1 1 1 2 2 2 3 3 4 0 0 0 0 1 1 1 2 2 3 0 0 0 1 1 2 0 0 1 0;];



ett =  build_multiplication_matrix( MONS6, MONS7, [0;0;0] );
v1 =  build_multiplication_matrix( MONS6, MONS7, [1;0;0] );
v2 =  build_multiplication_matrix( MONS6, MONS7, [0;1;0] );
v3 =  build_multiplication_matrix( MONS6, MONS7, [0;0;1] );
[dummy, I] = sort( MONS7(k,:) ); 
MONSt = MONS7(:,I);

switch k 
 case 1 
  M1 = [M*ett;M*v2; M*v3];
  MONStt = MONSt - [1;0;0]*ones(1,120);
  vv=v1;
 case 2 
  M1 = [M*ett;M*v1; M*v3];
  MONStt = MONSt - [0;1;0]*ones(1,120);
  vv = v2;
 case 3 
  M1 = [M*ett;M*v1; M*v2];
  MONStt = MONSt - [0;0;1]*ones(1,120);
  vv=v3;
end
mydiv =  build_multiplication_matrix( MONStt, MONS6, [0;0;0] );
    
M2 = M1(:,I);
% tt = null( M2(:,1:35)' );
[U,S,V] = svd(M2(:,1:35)',0);
switch k
 case 1
  tt=  V(:,end-9:end) ;
  M3 =  tt'*M2;
  M3 = M3(1:9,:);
 case {2,3}
  tt=  V(:,end-16:end) ;
  M3 =  tt'*M2;
  M3 = M3(1:16,:);
 otherwise
  error('här')
end



M4 = M3*mydiv;
t1 = M4*vv;
% max(max(t1(:,I)- M3))


newM = M4;



function M = build_multiplication_matrix( MONS_from, MONS_to, mult )
mult = mult(:)';
M =[];
for i=1:size(MONS_from,2)
   p = MONS_from(:,i)';
   M=[M ismember(  MONS_to', p+mult , 'rows' ) ];
end
M = M';