clear all
clc


% status = system('mkdir mynew')
% command = './build-cmake/src/dune-microstructure ./inputs/cellsolver.parset';
% system(['set PATH=' '/home/klaus/Desktop/DUNE/dune-microstructure' ';' command ]);

% --- 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( 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+sqrt(2)*q3*b3*v1*v2)


% ---- 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)


% Substitute effective quantitites
% 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));  
% % f_minus = subs(f_minus,b3,0);         

% 2. Substitute specific values:

% Compare with 'ClassifyMin-Code'

% Compare with 'ClassifyMin-Code'
mu_1 = 1;
rho_1 = 1;
% --- type 1 Situation:
% beta = 2;
% alpha = 2;
% theta = 1/4;
% --- type 2 Situation:
% beta = 3.0714;
% alpha = -20;
% theta = 0.3;
% --- type 3 Situation:
% beta = 2.2857;
% alpha = -20;
% theta = 0.3;

% interesting: 
alpha = 18.3673;
beta = 8.57143;
theta= 0.913265;

% alpha = 2.85714;
% beta = 7;
% theta= 0.3;



set_mu_gamma = 'q1';
% set_mu_gamma = 'q2';
print_output = false;

q1i = mu_1.*(beta./(theta+(1-theta).*beta))
q2i = mu_1.*((1-theta)+theta.*beta)
q3i = q1i
% b1i = (mu_1*rho_1/4).*(beta./(theta+(1-theta).*beta)).*(1-theta.*(1+alpha))
% b2i =  mu_1.*(rho_1/8).*(1-theta.*(1+beta.*alpha))
b3i = 0

%TEST (new)
b1i = (3*rho_1/2).*beta.*(1-theta.*(1+alpha));
b2i = (3*rho_1/(4*((1-theta)+theta.*beta))).*(1-theta.*(1+beta.*alpha));

f_plus = subs(f_plus,q1,q1i);
f_plus = subs(f_plus,q3,q3i);
f_plus = subs(f_plus,q2,q2i);
f_plus = subs(f_plus,b1,b1i);
f_plus = subs(f_plus,b2,b2i);
f_plus = subs(f_plus,b3,b3i)

f_minus = subs(f_minus,q1,q1i);
f_minus = subs(f_minus,q3,q3i);
f_minus = subs(f_minus,q2,q2i);
f_minus = subs(f_minus,b1,b1i);
f_minus = subs(f_minus,b2,b2i);
f_minus = subs(f_minus,b3,b3i)



[A,angle,V] = classifyMIN(mu_1,rho_1,alpha,beta,theta,set_mu_gamma,print_output)

%

% Substitute random values...
% 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);

% 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: 

% More robust (works even for values b_3 ~ 1e-08 ): 
S = solve(eqns,v1,v2,'MaxDegree' , 5);  
S_minus = solve(eqns_minus,v1,v2,'MaxDegree' , 5);

%Tests:
% 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, '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)

%---------- TEST if Grad(f) = 0 ---------------------
% fprintf('Testing equation grad(f) = 0  with stationary points')
% 
% 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
% ------------------------------------

fprintf('stationary points of f_plus:')
A = double(A)                                     %safe symbolic values
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]

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]

% 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,:);
    sign = 1.0;
elseif GlobalIdx == 2  %Min_minus 
    GlobalMinimizer = SP_Minus(MinIdx_minus,:);
    sign = -1.0;
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);

%
%
% Compare with 'ClassifyMin-Code'

fprintf('----------------compare with ClassifyMIN----------------')
fprintf('Output Minimizer Matrix from symbolic Minimization')

sign*GlobalMinimizer'*GlobalMinimizer %sign correct?   should do this with symbolic Values! TODO
% GlobalMinimizer'*GlobalMinimizer

%check with higher Precision:
% vpa(GlobalMinimizer'*GlobalMinimizer)



%Output from Classify Min:
[A,angle,type,K] = classifyMIN(mu_1,rho_1,alpha,beta,theta,set_mu_gamma,print_output);
fprintf('Output Minimizer Matrix from ClassifyMIN')

% [A(1) sign*sqrt(A(1)*A(2)) ; sign*sqrt(A(1)*A(2)) A(2)]  %sign correct?
[A(1) sqrt(A(1)*A(2)) ; sqrt(A(1)*A(2)) A(2)]  %sign correct?

%check with higher Precision:
% vpa([A(1) sqrt(A(1)*A(2)) ; sqrt(A(1)*A(2)) A(2)])


e = [sqrt(A(1)), sqrt(A(2))];      %TODO .. this might be complex?!

norm = sqrt((A(1)+A(2)));

e = e./norm;

K*(e'*e)

% e'*e
% K

fprintf('angle: %d', angle)
fprintf('Type: %d', type)


%
% Compare with "Task2"

fprintf('----------------compare with Task2----------------')

B = [b1i b3i; b3i b2i];
x = 0:0.01:2*pi;

y1 = arrayfun(@(alpha)compute_F(alpha,B,q1i,q2i,q3i),x,'UniformOutput', false);
y1 = cell2mat(y1);


figure 
plot(x,y1,'b')
hold on 

fun = @(a) compute_F(a,B,q1i,q2i,q3i);
[alphaMin,f] = fminunc(fun,0)
[alphaMin,f] = fminunc(fun,3)        % Different initial value
plot(alphaMin,f,'*')

%compute radius
rMin = compute_r(alphaMin,B,q1i,q2i,q3i)

%compute Minimizer:
v_alpha = [cos(alphaMin);sin(alphaMin)];



G = rMin.*(v_alpha*v_alpha')

%
% Determine Minimizer Type (in general case)

T = [GlobalMinimizer' e1']
det(T)  % also works?
 

% symbolically : 

if GlobalIdx == 1     %Min_plus
    A_sym = S.v1;
    B_sym = S.v2
    Index = MinIdx_plus;
elseif GlobalIdx == 2  %Min_minus 
    A_sym = S_minus.v1;
    B_sym = S_minus.v2;
    Index = MinIdx_minus;
end

% Check Determinant symbolically?!?! 

g_sym = [A_sym(Index) B_sym(Index)]
G_sym = g_sym'*g_sym

e1 = [1 0];
e2 = [0 1];

% check alignment with e1
% if .... 
det([g_sym' e1'])
% ... bending in e1 direction
% check alignment with e2
% if..
det([g_sym' e2'])
double(det([g_sym' e2']))
% bending in e2 direction
%Else 
%.... 