clear all
clc

% --- Change PolynomialDisplayStyle ----
% sympref('PolynomialDisplayStyle','ascend');
% sympref('AbbreviateOutput',false);

syms f_plus(v1,v2,q1,q2,q3,b1,b2,b3)
assume( q1 > 0)
assume( q2 > 0)
assume( q3 > 0)
% assume(q3 == q1);
assume(q3 >= q1)
assume(q2 >= q3)


v = [v1; v2];

%should be sqrt(2) instead of 2!
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+sqrt(2)*q3*b3*v1*v2);


% ---- Fix parameters
% f_plus = subs(f_plus,b3,0)  % set b3
% f_plus = subs(f_plus,q3,q1)

% f_plus = subs(f_plus,q1,40);
% f_plus = subs(f_plus,q3,63.9);
% f_plus = subs(f_plus,q2,34.9);
% f_plus = subs(f_plus,b1,4);
% f_plus = subs(f_plus,b2,2.4);
% f_plus = subs(f_plus,b3,2.4);

% f_plus = subs(f_plus,q1,40);
% f_plus = subs(f_plus,q3,20);
% f_plus = subs(f_plus,q2,50);
% f_plus = subs(f_plus,b1,5);
% f_plus = subs(f_plus,b2,6);
% f_plus = subs(f_plus,b3,7);

f_plus = subs(f_plus,q1,40);
f_plus = subs(f_plus,q3,63.9);
f_plus = subs(f_plus,q2,70);
rndm1 = randi([1 20],1,1);
rndm2 = randi([1 20],1,1);
rndm3 = randi([1 20],1,1);
f_plus = subs(f_plus,b1,rndm1);
f_plus = subs(f_plus,b2,rndm2);
f_plus = subs(f_plus,b3,rndm3);


f_minus = subs(f_minus,q1,40);
f_minus = subs(f_minus,q3,63.9);
f_minus = subs(f_minus,q2,70);
f_minus = subs(f_minus,b1,rndm1);
f_minus = subs(f_minus,b2,rndm2);
f_minus = subs(f_minus,b3,rndm3);


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

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


eq1 = df_plusx == 0;
eq2 = df_plusy == 0;
eqns = [eq1, eq2]

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


S = solve(eqns,v1,v2,'MaxDegree' , 5, 'Real', true);
S_minus = solve(eqns_minus,v1,v2,'MaxDegree' , 5, 'Real', true);
% S = solve(eqns,v1,v2,'MaxDegree' , 5);
% 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)
% S_minus = solve(eqns_minus,v1,v2);
% S_minus.v1
% S_minus.v2

%---------- TEST ---------------------
% r = subs(subs(df_plusx,v1,A(3)),v2,B(3));
fprintf('Testing equation grad(f) = 0  with stationary points')
double(subs(subs(df_plusx,v1,A(1)),v2,B(1)))
double(subs(subs(df_plusx,v1,A(2)),v2,B(2)))
double(subs(subs(df_plusx,v1,A(3)),v2,B(3)))
% ------------------------------------





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


% determine global Minimizer from stationary points:
fprintf('function values at stationary points:')
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')


% determine global Minimizer from stationary points:
fprintf('function values at stationary points:')
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')

globalMinimizerValue = min(Min_plus,Min_minus)

% Plot function
fsurf(@(x,y) f_plus(x,y,q1,q2,q3,b1,b2,b3))
hold on 
plot3(A,B,T,'g*')
% 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,'g*')





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