% fit a simple unrotaded ellipse (1/a^2)x1^2 +
% (1/b^2)x2^2 = 1 with the least squares solution
clear all, close all,clc
% load some data
load ..\yamashita\stiction_model\all_FIC_loops_nominal.mat
op = OP_coll{85};mv = MV_coll{85};
op = op - mean(op);  mv = mv - mean(mv);
x1 = op; x2 = mv/25*100;

% Solve the system that desribed the least squares fit
% of the ellipse using the variable theta = [1/a^2 1/b^2]

% A = [x1.^2 x2.^2];
% b = ones(length(x1),1);
% theta = (A'*A)\(A'*b);
% a = sqrt(1/theta(1))
% b = sqrt(1/theta(2))

% plot the data and the fitted ellipse

plot(x1,x2); 
hold
% x = -a:a/100:a;
% plot(x,b*sqrt(1-theta(1)*x.^2),'r-o')
% plot(x,-b*sqrt(1-theta(1)*x.^2),'r-o')
% x_stored = x; b_stored = b;

% step two. Try to fit a rotated and possibly
% translated ellipse.

A2 = [x1.^2 x2.^2 x1.*x2 x1 x2];
b2 = ones(length(x1),1);
theta2 = (A2'*A2)\(A2'*b2);
norm(A2*theta2 - b2)

% Interpret the results
a1 = theta2(1); a2 = theta2(2); a12 = theta2(3);
clear b A
b = [theta2(4) theta2(5)]'; c = -1;
A = [a1 0.5*a12; 0.5*a12 a2];
t = -0.5*inv(A)*b;
c_bar = t'*A*t + b'*t + c;
[Q eigval] = eig(A); % Q = Q';
eigval = [eigval(1,1) eigval(2,2)]';
m = sqrt(-c_bar/eigval(1));
n = sqrt(-c_bar/eigval(2));

% plot the rotated and translated ellipse
% a) draw the ellipse in the new coordinates
x1bar = -m:m/100:m;
x2bar1 =  n*sqrt(1-(x1bar/m).^2);
x2bar2 = -n*sqrt(1-(x1bar/m).^2);

x_bar1 = [x1bar ; x2bar1];
x_OK1 = Q*x_bar1 + [t(1)*ones(1,length(x_bar1)); t(2)*ones(1,length(x_bar1))] ;
x_bar2 = [x1bar ; x2bar2];
x_OK2 = Q*x_bar2 + [t(1)*ones(1,length(x_bar1)); t(2)*ones(1,length(x_bar1))];

plot(x_OK1(1,:),x_OK1(2,:),'c-d');
% plot(x_OK2(1,:),x_OK2(2,:),'c-d');
x_OK2_stored = x_OK2;

axis square

% last step. Use optimization find find an even better
% ellipse

options=optimset('Display','iter','MaxFunEvals',1e6,'MaxIter',1e6);

%     X=FMINCON(FUN,X0,A,B,Aeq,Beq,LB,UB,NONLCON)
% find_c0 = inline('(1 - norm([theta2'' c]))^2','c','theta2');
% c0 = fminsearch(@(c)find_c0(c,theta2),100)
theta3=fmincon(@(theta3)ellipse_fmincon(theta3,x1,x2),[theta2' -1],[],...
    [],[],[],[],[],@nonlincon_ellipse,options);
disp(['norm (theta3) = ' num2str(norm(theta3))]);

a1 = theta3(1); a2 = theta3(2); a12 = theta3(3);
clear b A
b = [theta3(4) theta3(5)]'; c = theta3(6);
A = [a1 0.5*a12; 0.5*a12 a2];
t = -0.5*inv(A)*b;
c_bar = t'*A*t + b'*t + c;
[Q eigval] = eig(A);
% Q = Q';
eigval = [eigval(1,1) eigval(2,2)]';
m = sqrt(-c_bar/eigval(1));
n = sqrt(-c_bar/eigval(2));

% plot the rotated and translated ellipse
% a) draw the ellipse in the new coordinates
clear x1bar x2bar1 x2bar2 x_bar1 x_OK1 x_bar2 x_OK2
x1bar = -m:m/100:m;
x2bar1 =  n*sqrt(1-(x1bar/m).^2);
x2bar2 = -n*sqrt(1-(x1bar/m).^2);

x_bar1 = [x1bar ; x2bar1];
x_OK1 = Q*x_bar1 + [t(1)*ones(1,length(x_bar1)); t(2)*ones(1,length(x_bar1))] ;
x_bar2 = [x1bar ; x2bar2];
x_OK2 = Q*x_bar2 + [t(1)*ones(1,length(x_bar1)); t(2)*ones(1,length(x_bar1))];

plot(x_OK1(1,:),x_OK1(2,:),'y-o');
% plot(x_OK2(1,:),x_OK2(2,:),'y-o');

% legend('Centered experimental data','Simple unrotated ellipse',...
%     'Unconstrained least squares','Constrained optimization')
legend('Experimental data',...
    'Unconstrained least squares','Constrained optimization')

% plot(x_stored,-b_stored*sqrt(1-theta(1)*x.^2),'r-o')
plot(x_OK2_stored(1,:),x_OK2_stored(2,:),'c-d');
plot(x_OK2(1,:),x_OK2(2,:),'y-o');
