function result = full_calibration(data)
% full\_calibration(data): calibrates odometry and sensor 
%   data.phi\_l(k), data.phi\_r(k) : Measured wheel speed
%           data.sm(:,k)      : Scan matching data
%   data.T(k)  : time interval

result = data;

% Find $m_3$ and $m_4$ by standard least squares
A = zeros(2,2); g = zeros(2,1);
n = size(data.phi_r,2);
for k=1:n
	L_k = data.T(k) * [data.phi_l(k) data.phi_r(k)];
	y_k = data.sm(3, k);
	A = A + L_k' * L_k;
	g = g + L_k' * y_k;
end

if cond(A) > 50
	fprintf('Matrix A is almost singular. Not very exciting data!\n')
	return
end

x = A \ g;
m3 = x(1); m4 = x(2);

if isnan(m3) || isnan(m4)
	A
	g
	error('Could not find first two parameters m3, m4')
end

% Find the other parameters
M = zeros(5,5); 
M2 = zeros(6,6);
for k=1:n
	sm_k = data.sm(:,k);
	T = data.T(k);
	o_theta = T * (m3 * data.phi_l(k) + m4 * data.phi_r(k));
	%w0 = sm_k(3) / T;
	w0 = o_theta / T;
	
	c3 = 0.5 * T * ...
		(-m3 * data.phi_l(k) + m4 * data.phi_r(k));
	if abs(w0*T) > 1e-7
		c4 = c3 * (   sin(w0*T)   / (w0*T) );
		c5 = c3 * ( (1-cos(w0*T)) / (w0*T) );	
	else
		c4 = c3 * 1;
		c5 = c3 * 0;
	end

	% better condition
	%c4 = c4 * 1000;
	%c5 = c5 * 1000;
	
	L_k = [  -c4,  1-cos(w0*T), +sin(w0*T),   ...
	           +sm_k(1), -sm_k(2)                 ; ...
	         -c5,  -sin(w0*T), 1-cos(w0*T),   ...
	           +sm_k(2), +sm_k(1)                 ];
	M   = M + L_k' * L_k;

	con = 1;
	c6 = con * 0.5 * T * (-m3 * data.phi_l(k)) * (sin(w0*T) / (w0*T));
	c7 = con * 0.5 * T * (+m4 * data.phi_r(k)) * (sin(w0*T) / (w0*T));
	c8 = con * 0.5 * T * (-m3 * data.phi_l(k)) * ((1-cos(w0*T)) / (w0*T));
	c9 = con * 0.5 * T * (+m4 * data.phi_r(k)) * ((1-cos(w0*T)) / (w0*T));
	
	L2_k = [  c6, c7, 1-cos(w0*T), +sin(w0*T),   ...
	           +sm_k(1), -sm_k(2)                 ; ...
	         c8, c9,  -sin(w0*T), 1-cos(w0*T),   ...
	           +sm_k(2), +sm_k(1)                 ];
	M2   = M2 + L2_k' * L2_k;
end
result.M2 = M2;


% option 1: minimize in closed form 
%
x_closed_form = full_calibration_min(M);

% option 2: start at the minimum eigenvalue, find numerical solution
%
[e, v] = eig(M);
v = e(:,1); % eigenvector for smallest eigenvalue
x = v * ( sign(v(1)) / norm(v(4:5)) );

% check with a numerical method it's really the minimum
options = optimset(optimset('fminsearch'), 'TolX', 1e-12, 'TolFun', 1e-12);
f = @(z) [z(1) z(2) z(3) cos(z(4)) sin(z(4))] * M * [z(1) z(2) z(3) cos(z(4)) sin(z(4))]';
z0 = [x(1) x(2) x(3) atan2(x(5),x(4))];
[x_num,fval,exitflag,output] = fminsearch(f, z0, options);
x_num(5) = sin(x_num(4)); x_num(4) = cos(x_num(4));


use_x = x_closed_form;
use_x = x_num;

result.est_d = use_x(1);
result.est_r_l = -use_x(1) * m3;
result.est_r_r = +use_x(1) * m4;
result.est_laser = [use_x(2);use_x(3);atan2(use_x(5),use_x(4))]; 

result.est_params = [result.est_d;result.est_r_l;result.est_r_r;result.est_laser];
result.M = M;
result.x = x';
result.x_num = x_num;
result.x_closed = x_closed_form';
result.A = A; 
result.g = g;
result.m3 = m3; 
result.m4 = m4; 
result.est_E_d = result.est_r_r / result.est_r_l; 
result.est_E_b = result.est_d / 0.09;

ev = eig(M);
result.error_measure = ev(1) / ev(2);


[e, v] = eig(M2);
v = e(:,2); % eigenvector for second eigenvalue
y = v * ( sign(v(1)) / norm(v(4:5)) );

% check with a numerical method it's really the minimum
options = optimset(optimset('fminsearch'), 'TolX', 1e-12, 'TolFun', 1e-12);
fy = @(z) [z(1) z(2) z(3) z(4) cos(z(5)) sin(z(5))] * M2 * [z(1) z(2) z(3) z(4) cos(z(5)) sin(z(5))]';
y0 = [y(1) y(2) y(3) y(4) atan2(y(6),y(5))];
[y_num,fval,exitflag,output] = fminsearch(fy, y0, options);
y_num(6) = sin(y_num(5)); y_num(5) = cos(y_num(5));

result.y = y';
result.y_num = y_num;