clear
clc

% Load spectro and WL+GCph+XC
FISH_GCsp = load('Fisher_GCsp_flat');
FISH_XC = load('Fisher_XC_GCph_WL_flat');

index_reorder = [3 1 5 6 2 4 7];

% Reorder cosmo parameters
FISH_GCsp(1:7,:) = FISH_GCsp(index_reorder,:);
FISH_GCsp(:,1:7) = FISH_GCsp(:,index_reorder);

% Marginalizing over uncommon parameters between the two matrices
COV_GCsp_first = inv(FISH_GCsp);
COV_XC_first = inv(FISH_XC);
COV_GCsp = COV_GCsp_first(1:7,1:7);
COV_XC = COV_XC_first(1:7,1:7);

% Invert to get Fisher matrix
FISH_sp = inv(COV_GCsp);
FISH_xc = inv(COV_XC);

% Get eigen values (marginalized error since we handle Covariance error)
% and eigen vectors giving matrix "P" with convention : F = P D P^-1
[eigenv_sp, eigen_sp] = eig(COV_GCsp);
[eigenv_xc, eigen_xc] = eig(COV_XC);

% Fisher eigen scalar vectors
FISH_eigen_sp = inv(eigen_sp);
FISH_eigen_xc = inv(eigen_xc);

% Eigen sum for  Fisher matrices
FISH_eigen_sum = FISH_eigen_sp + FISH_eigen_xc;
% Eigen sum matrix for  Fisher matrices
%FISH_eigenv_sum = FISH_eigenv_sp + FISH_eigenv_xc

% Fisher matrices
F1 = FISH_sp;
F2 = FISH_xc;
P1 = eigenv_sp;
P2 = eigenv_xc;
D1 = FISH_eigen_sp;
D2 = FISH_eigen_xc;

%% unknown variables
a = sym('a_',[7,7]);
b = sym('b_',[7,7]);

%% generate function from equations
D = (eye(7).*diag(a).^2)*D1 + (eye(7).*diag(b).^2)*D2;
%D = a*a*D1 + b*b*D2;
%D = (a.*a)*D1 + (b.*b)*D2;
%Eq1 = a*a + a*P1*b*(P2.') + b*P2*a*(P1.') + b*b - eye(7);
%Eq1 = a*P1*a*(P1.') + a*P1*b*(P2.') + b*P2*a*(P1.') + b*P2*b*(P2.') - eye(7);
Eq1 = (P1.')*(a.')*a*P1 + (P1.')*(a.')*b*P2 + (P2.')*(b.')*a*P1 + (P2.')*(b.')*b*P2 - eye(7);
Eq2 = F1*a*P1 + F1*b*P2 + F2*a*P1 + F2*b*P2 - (a*P1 + b*P2)*D;
Eqs = reshape([Eq1,Eq2],2*7*7,[]);
%Eqs = reshape([Eq1,Eq2],[1,98]);
%Fun = matlabFunction(Eqs);

% Solution
%Eqs_vars = num2cell(symvar(Eqs));
Eqs_vars = {[a(:), b(:)].'};
F = matlabFunction(Eqs(:), 'var', Eqs_vars);

%F = @(x) Fun(...
%    x(	1	),	x(	2	),	x(	3	),	x(	4	),	x(	5	),	x(	6	),	x(	7	),...
%    x(	8	),	x(	9	),	x(	10	),	x(	11	),	x(	12	),	x(	13	),	x(	14	),...
%    x(	15	),	x(	16	),	x(	17	),	x(	18	),	x(	19	),	x(	20	),	x(	21	),...
%    x(	22	),	x(	23	),	x(	24	),	x(	25	),	x(	26	),	x(	27	),	x(	28	),...
%    x(	29	),	x(	30	),	x(	31	),	x(	32	),	x(	33	),	x(	34	),	x(	35	),...
%    x(	36	),	x(	37	),	x(	38	),	x(	39	),	x(	40	),	x(	41	),	x(	42	),...
%    x(	43	),	x(	44	),	x(	45	),	x(	46	),	x(	47	),	x(	48	),	x(	49	),...
%    x(	50	),	x(	51	),	x(	52	),	x(	53	),	x(	54	),	x(	55	),	x(	56	),...
%    x(	57	),	x(	58	),	x(	59	),	x(	60	),	x(	61	),	x(	62	),	x(	63	),...
%    x(	64	),	x(	65	),	x(	66	),	x(	67	),	x(	68	),	x(	69	),	x(	70	),...
%    x(	71	),	x(	72	),	x(	73	),	x(	74	),	x(	75	),	x(	76	),	x(	77	),...
%    x(	78	),	x(	79	),	x(	80	),	x(	81	),	x(	82	),	x(	83	),	x(	84	),...
%    x(	85	),	x(	86	),	x(	87	),	x(	88	),	x(	89	),	x(	90	),	x(	91	),...
%    x(	92	),	x(	93	),	x(	94	),	x(	95	),	x(	96	),	x(	97	),	x(	98	));
%
%% solving
%options = optimoptions('fmincon','Display','iter','PlotFcn',@optimplotfirstorderopt);
%options = optimoptions('fmincon')
%options.MaxFunctionEvaluations = 1e10;
%options.MaxIterations = 1e5;

%x0 =  ones(2*7*7,1);     %  initial point
x0 =  [ones(1,98)];     %  initial point
%x0 = [reshape(eigenv_sp,[1,49]), reshape(eigenv_xc,[1,49])]
%x0 = [mean(diag(eigen_sp))*ones(49), mean(diag(eigen_xc))*ones(49)]

options = optimoptions('fmincon','Algorithm', 'interior-point',...
    'UseParallel',true, 'Display','off','MaxFunctionEvaluations',6000, 'TolCon', 1e-4, 'TolFun', 1e-4);
problem = createOptimProblem('fmincon', ...
'objective',F, ...
'x0',x0, ...
'lb',[-1e3*ones(1,98)], ...
'ub',[1e3*ones(1,98)], ...
'options',options);
gs = GlobalSearch;
[x, fval] = run(gs,problem);

%x0 = reshape([eigenv_sp, eigenv_xc],[7, 7, 2])
%x0 = reshape(x0(:), [7,7,2]);  %  initial point
%x0 = horzcat(x0{:})
%[x,fval] = fsolve(F,x0)
%[x,fval] = fsolve(F,x0,options)

%% output
a = zeros(7,7);
b = zeros(7,7);
%x = reshape(x,[2,7,7]);
for ii=1:7
    a(ii,:) = x(1+7*(ii-1):7*ii);
    b(ii,:) = x(50+7*(ii-1):49+7*ii);
end

disp('a*transpose(a) = ')
a*a'
disp('b*transpose(b) = ')
b*b'

dlmwrite('a_normal.txt',a,'delimiter',' ')
dlmwrite('b_normal.txt',b,'delimiter',' ')

% Sum of passing matrix P = aX + bY with X = eigenv_sp, Y = eigenv_xc 
% a and b to infer
eigenv_final = a*eigenv_sp + b*eigenv_xc

% Fisher final
FISH_final = eigenv_final*FISH_eigen_sum*(eigenv_final.');

% Save Fisher_final
dlmwrite('Fisher_final.txt',FISH_final,'delimiter',' ')

%status = system('/Users/fab/bin/printFoM_and_Constraints_from_Fisher Fisher_final.txt OPT GCph 0 0 F N');

