clear all
clc

% f_plus = q1*v1^4 + q2*v2^4+2*q3*v1^2*v2^2-2*(q1*b1*v1^2+ q2*b2*v2^2+2*q3*b3*v1*v2)
syms f_plus(v1,v2,q1,q2,q3,b1,b2,b3)
assume( q1 > 0)
assume( q2 > 0)
assume( q3 > 0)
assume( q3 >= q1)
assume( q2 >= q3)

v = [v1; v2];

%should be sqrt(2) instead of 2
fprintf('Functions to be minimized')
f_plus(v1,v2,q1,q2,q3,b1,b2,b3) = q1*v1^4 + q2*v2^4+2*q3*v1^2*v2^2-2*(q1*b1*v1^2+ q2*b2*v2^2+sqrt(2)*q3*b3*v1*v2)
f_minus(v1,v2,q1,q2,q3,b1,b2,b3) = q1*v1^4 + q2*v2^4+2*q3*v1^2*v2^2+2*(q1*b1*v1^2+ q2*b2*v2^2+2*q3*b3*v1*v2)


% Compute the Gradients 
df_plusx = diff(f_plus,v1);
df_plusy = diff(f_plus,v2);

df_minusx = diff(f_minus,v1);
df_minusy = diff(f_minus,v2);

% Setup Equations Grad(f) = 0 
eq1 = df_plusx == 0;
eq2 = df_plusy == 0;
eqns = [eq1, eq2]

eq3 = df_minusx == 0;
eq4 = df_minusy == 0;
eqns_minus = [eq3, eq4]


% Symbolically Solve Equations: 

S = solve(eqns,v1,v2,'MaxDegree' , 5);
S_minus = solve(eqns_minus,v1,v2,'MaxDegree' , 5);

%Test
% S = solve(eqns,v1,v2,'MaxDegree' , 5, 'IgnoreAnalyticConstraints',true, 'Real', true);  %does not work...
% S_minus = solve(eqns_minus,v1,v2,'MaxDegree' , 5, 'IgnoreAnalyticConstraints',true, 'Real', true); %does not work...
% S = solve(eqns,v1,v2,'MaxDegree' , 5, 'Real', true);  %does not work...
% S_minus = solve(eqns_minus,v1,v2,'MaxDegree' , 5, 'Real', true); %does not work...
% S = solve(eqns,v1,v2,'MaxDegree' , 5, 'Real', true);
% S = solve(eqns,v1,v2,'MaxDegree' , 5, 'IgnoreAnalyticConstraints',true);
% S = solve(eqns,v1,v2,'MaxDegree' , 5, 'IgnoreAnalyticConstraints',true, 'Real', true);
% S = solve(eqns)

A = S.v1;
B = S.v2;
A_minus = S_minus.v1;
B_minus = S_minus.v2;
% A = simplify(A);
% B = simplify(B)

%
% Substitue Values for q1,q2,q3,b1,b2,b3

% ---- Fix parameters

% 1. Import effective quantities from CellSolver-Code:
Qmat = spconvert(load('QMatrix.txt'));
Qmat = full(Qmat)

Bmat = spconvert(load('BMatrix.txt'));
Bmat = full(Bmat)

B = subs(B,Qmat(1,1));
B = subs(B,q3,Qmat(3,3));
B = subs(B,q2,Qmat(2,2));
B = subs(B,b1,Bmat(1));
B = subs(B,b2,Bmat(2));
B = subs(B,b3,Bmat(3));

A = subs(A,q1,Qmat(1,1));
A = subs(A,q3,Qmat(3,3));
A = subs(A,q2,Qmat(2,2));
A = subs(A,b1,Bmat(1));
A = subs(A,b2,Bmat(2));
A = subs(A,b3,Bmat(3));

B_minus = subs(B_minus,Qmat(1,1));
B_minus = subs(B_minus,q3,Qmat(3,3));
B_minus = subs(B_minus,q2,Qmat(2,2));
B_minus = subs(B_minus,b1,Bmat(1));
B_minus = subs(B_minus,b2,Bmat(2));
B_minus = subs(B_minus,b3,Bmat(3));

A_minus = subs(A_minus,q1,Qmat(1,1));
A_minus = subs(A_minus,q3,Qmat(3,3));
A_minus = subs(A_minus,q2,Qmat(2,2));
A_minus = subs(A_minus,b1,Bmat(1));
A_minus = subs(A_minus,b2,Bmat(2));
A_minus = subs(A_minus,b3,Bmat(3));

% Substitute effective quantitites in functions
f_plus = subs(f_plus,q1,Qmat(1,1));
f_plus = subs(f_plus,q3,Qmat(3,3));
f_plus = subs(f_plus,q2,Qmat(2,2));
f_plus = subs(f_plus,b1,Bmat(1));
f_plus = subs(f_plus,b2,Bmat(2));
f_plus = subs(f_plus,b3,Bmat(3));
% f_plus = subs(f_plus,b3,0);

f_minus = subs(f_minus,q1,Qmat(1,1));
f_minus = subs(f_minus,q3,Qmat(3,3));
f_minus = subs(f_minus,q2,Qmat(2,2));
f_minus = subs(f_minus,b1,Bmat(1));
f_minus = subs(f_minus,b2,Bmat(2));
f_minus = subs(f_minus,b3,Bmat(3));  %does not work ...
% f_minus = subs(f_minus,b3,0);          % works...

fprintf('stationary points of f_plus:')
A = double(A)
B = double(B)
fprintf('stationary points of f_minus:')
A_minus = double(A_minus)
B_minus = double(B_minus)

% Extract only Real-Solutions
fprintf('real stationary points of f_plus:')
tmp1 = A(imag(A)==0 & imag(B) == 0);
tmp2 = B(imag(A)==0 & imag(B) == 0);
A = tmp1;
B = tmp2;
% A(abs(imag(A)) <1e-3  & abs(imag(B)) <1e-3 )
SP_Plus = [A,B]

% Extract only Real-Solutions
fprintf('real stationary points of f_minus:')
tmp1 = A_minus(imag(A_minus)==0 & imag(B_minus) == 0);
tmp2 = B_minus(imag(A_minus)==0 & imag(B_minus) == 0);
A_minus = tmp1;
B_minus = tmp2;
% A_minus(abs(imag(A_minus)) <1e-3  & abs(imag(B_minus)) <1e-3 )
SP_Minus = [A_minus,B_minus]

% 2. Substitute specific values:
% B = subs(B,q1,40);
% B = subs(B,q3,20);
% B = subs(B,q2,50);
% B = subs(B,b1,5);
% B = subs(B,b2,6);
% B = subs(B,b3,7);
% A = subs(A,q1,40);
% A = subs(A,q3,20);
% A = subs(A,q2,50);
% A = subs(A,b1,5);
% A = subs(A,b2,6);
% A = subs(A,b3,7);

%---------- TEST if Grad(f) = 0 ---------------------
fprintf('Testing equation grad(f) = 0  with stationary points')

%recompute the gradients after substitution
df_plusx = diff(f_plus,v1);
df_plusy = diff(f_plus,v2);
df_minusx = diff(f_minus,v1);
df_minusy = diff(f_minus,v2);

for i = 1:size(A,1)
    fprintf('Testing %d.point (f_plus): ',i )
    [ double(subs(subs(df_plusx,v1,A(i)),v2,B(i))), double(subs(subs(df_plusy,v1,A(i)),v2,B(i))) ]
end
for i = 1:size(A_minus,1)
    fprintf('Testing %d.point (f_minus): ',i )
    [double(subs(subs(df_minusx,v1,A_minus(i)),v2,B_minus(i))), double(subs(subs(df_minusy,v1,A_minus(i)),v2,B_minus(i)))]
end
% ------------------------------------


% Determine global Minimizer from stationary points:
fprintf('function values at stationary points (f_plus):')
T = arrayfun(@(v1,v2) double(f_plus(v1,v2,q1,q2,q3,b1,b2,b3)),A,B,'UniformOutput', false)
T = cell2mat(T);
% Min_plus = min(T, [], 'all')
[Min_plus,MinIdx_plus] = min(T, [], 'all', 'linear')  

fprintf('function values at stationary points (f_minus):')
T_minus = arrayfun(@(v1,v2) double(f_minus(v1,v2,q1,q2,q3,b1,b2,b3)),A_minus,B_minus,'UniformOutput', false)
T_minus = cell2mat(T_minus);
% Min_minus = min(T_minus, [], 'all')
[Min_minus,MinIdx_minus] = min(T_minus, [], 'all', 'linear')   

[globalMinimizerValue,GlobalIdx] = min([Min_plus,Min_minus])
 
if GlobalIdx == 1     %Min_plus
    GlobalMinimizer = SP_Plus(MinIdx_plus,:);
elseif GlobalIdx == 2  %Min_minus 
    GlobalMinimizer = SP_Minus(MinIdx_minus,:);
end

fprintf('Global Minimizer:(%d,%d)', GlobalMinimizer(1),GlobalMinimizer(2) )
fprintf('Global Minimizer Value : %d', globalMinimizerValue )

% Plot functions
fsurf(@(x,y) f_plus(x,y,q1,q2,q3,b1,b2,b3))
hold on 
plot3(A,B,T,'g*')
%Plot GlobalMinimizer:
hold on 
plot3(GlobalMinimizer(1),GlobalMinimizer(2),globalMinimizerValue, 'o', 'Color','c')
% view(90,0)
% view(2)

figure
fsurf(@(x,y) f_minus(x,y,q1,q2,q3,b1,b2,b3))
hold on
plot3(A_minus,B_minus,T_minus,'g*')
hold on 
plot3(GlobalMinimizer(1),GlobalMinimizer(2),globalMinimizerValue, 'o', 'Color','c')


%Write to txt-File
fileID = fopen('txt.txt','w');
fprintf(fileID,'%s' , latex(S.v1));
fclose(fileID);