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];


% -------- Options ----------
print_Minimizer = true;
% print_statPoint = false;
print_statPoint = true;
% make_FunctionPlot = false;
make_FunctionPlot = true;
% print_Input = false;  %effective quantities
print_Input = true;
compare_with_Classification = true;





%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
absPath = "/home/klaus/Desktop/DUNE/dune-microstructure/outputs";

% 1. Import effective quantities from CellSolver-Code:
%read as sparse Matrix...
try %absolutePath
    Qmat = spconvert(load(absPath + '' + "/QMatrix.txt"));
    Bmat = spconvert(load(absPath + '' + "/BMatrix.txt"));
%     fprintf('Use absolute Path')
catch ME  % use relativePath
    Qmat = spconvert(load('../outputs/QMatrix.txt'));
    Bmat = spconvert(load('../outputs/BMatrix.txt'));
%     fprintf('Use relative Path')
end
%convert to full matrix... 
Qmat = full(Qmat);
Bmat = full(Bmat);

if print_Input
    fprintf('effective quadratic form:')
    Qmat
    fprintf('effective prestrain')
    Bmat
end


% check if Q is (close to..) symmetric
% könnte Anti-symmetric part berechnen und schauen dass dieser klein?
% Test: issymmetric(Qmat) does not work for float matrices?
% symmetric part 0.5*(Qmat+Qmat')
% anti-symmetric part 0.5*(Qmat-Qmat')
if norm(0.5*(Qmat-Qmat'),'fro') < 1e-10
    fprintf('Qmat (close to) symmetric')
else
    fprintf('Qmat not symmetric')
end



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

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

%

% 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_plus = [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_plus = solve(eqns_plus,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_plus = S_plus.v1;
B_plus = S_plus.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_plus(i)),v2,B_plus(i))), double(subs(subs(df_plusy,v1,A_plus(i)),v2,B_plus(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_plus = double(A_plus);             % should rather stay symbolic  
% B_plus = double(B_plus);             %much faster..?


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_plus(imag(A_plus)==0 & imag(B_plus) == 0);
% tmp2 = B_plus(imag(A_plus)==0 & imag(B_plus) == 0);
tmp1 = A_plus(imag(double(A_plus))==0 & imag(double(B_plus)) == 0);
tmp2 = B_plus(imag(double(A_plus))==0 & imag(double(B_plus)) == 0);
A_plus = tmp1;
B_plus = tmp2;
% A(abs(imag(A)) <1e-3  & abs(imag(B)) <1e-3 )
SP_plus = [A_plus,B_plus];

% 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);
tmp1 = A_minus(imag(double(A_minus))==0 & imag(double(B_minus)) == 0);
tmp2 = B_minus(imag(double(A_minus))==0 & imag(double(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];


% TODO one should use f_plus.subs(A_plus..) to compute function value
% symbolically?
% in the end only the stationaryPoints are used.. Ok to compare function
% values numerically

% Determine global Minimizer from stationary points:
% fprintf('function values at stationary points (f_plus):')
T_plus = arrayfun(@(v1,v2) double(f_plus(v1,v2,q1,q2,q3,b1,b2,b3)),A_plus,B_plus,'UniformOutput', false);
T_plus = cell2mat(T_plus);
% fprintf('size of T_plus:')
% size(T_plus)

%Test: rather use Substitution
% subs(f_plus,{v1, v2}, {A_plus,B_plus})


% 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);
% fprintf('size of T_minus:')
% size(T_minus)

%Test: rather use Substitution
% T_minus = subs(f_minus,{v1, v2}, {A_minus,B_minus})
% double(T_minus)

if print_statPoint
    fprintf('real stationary points of f_plus: ')
%     SP_Plus     %alternative: output as symbolic (can be unwieldy)
    double(SP_plus)
    fprintf('real stationary points of f_minus:')
%     SP_Minus     %alternative: output as symbolic (can be unwieldy)
    double(SP_minus)
    fprintf('function values at stationary points (f_plus):')
    T_plus
    fprintf('function values at stationary points (f_minus):')
    T_minus
end

% --- Find Stationary Point(s) with minimum Value 

% Min_plus = min(T, [], 'all')
[Min_plus,MinIdx_plus] = min(T_plus, [], 'all', 'linear');    %find one min...
% Min_minus = min(T_minus, [], 'all')
[Min_minus,MinIdx_minus] = min(T_minus, [], 'all', 'linear')
% [Min_minus,MinIdx_minus] = min(T_minus) %works with symbolic too

% Compare Minimizers of f_plus & f_minus
[globalMinimizerValue,GlobalIdx] = min([Min_plus,Min_minus]); %TODO could it be that f_plus/f_minus give same Minimizer??


if GlobalIdx == 1     %Min_plus   % i.e. Global Minimizer is given by f_plus
    GlobalMinimizer = SP_plus(MinIdx_plus,:);
    sign = 1.0;
elseif GlobalIdx == 2  %Min_minus % i.e. Global Minimizer is given by f_minus
    GlobalMinimizer = SP_minus(MinIdx_minus,:);
    sign = -1.0; 
end

% ------ Check if there are more SP with the same value...
 MinIndices_minus = find(T_minus(:) == globalMinimizerValue)    % Find indices of All Minima
 MinIndices_plus  = find(T_plus(:) == globalMinimizerValue)    % Find indices of All Minima
%     numMinSP = size(MinIndices,1)                       % This is always >= 2 due to the structure of the roots..
 numMinSP_minus = size(MinIndices_minus,1)   
 numMinSP_plus  = size(MinIndices_plus,1)
    
%     AllMinSP_minus = SP_minus(MinIndices_minus,:)
%     AllMinSP_minus = double(SP_minus(MinIndices_minus,:))
%     AllMin = T_minus(MinIndices)  %bereits klar dass diese selben funktionswert haben..
      
    Minimizer = sign*(GlobalMinimizer'*GlobalMinimizer); %global minimizing Matrix G*
    MinimizerCount = 1;
    % different Stationary Points might correspond to the same minimizing
    % Matrix G*... check this: 
    
    % compare only with other StationaryPoints/Minimizers
    % remove Index of Minimizer
    if GlobalIdx == 1     
        MinIndices_plus = MinIndices_plus(MinIndices_plus~=MinIdx_plus)
    elseif GlobalIdx == 2  
        MinIndices_minus = MinIndices_minus(MinIndices_minus~=MinIdx_minus)
    end
    
    MinIndices = cat(1,MinIndices_plus,MinIndices_minus)  %[Minimizers-Indices f_plus, Minimizer-Indices f_minus]
    
    for i = 1:(numMinSP_minus+numMinSP_plus-1)  % -1: dont count Minimizer itself.. 
        idx = MinIndices(i)   
        
        if i > numMinSP_plus
            SP = SP_minus(idx,:);
        else
            SP = SP_plus(idx,:); 
        end
        
%         SP_value = T_minus(idx) % not needed? 
        Matrix = sign*(SP'*SP);

        if norm(double(Matrix-Minimizer),'fro') < eps    %check is this sufficient here?
            % both StationaryPoints correspond to the same
            % (Matrix-)Minimizer
        else
            % StationaryPoint corresponds to a different (Matrix-)Minimizer
            MinimizerCount = MinimizerCount + 1;
        end
    end
% ----------------------------------------------------------------------------------------------------------------



% Output Uniqueness of Minimizers:
if MinimizerCount == 1
    fprintf('Unique Minimzier')
elseif MinimizerCount == 2
    fprintf('Two Minimziers')
else
    fprintf('1-Parameter family of Minimziers')
end



if print_Minimizer
    fprintf('Global Minimizer:(%d,%d)', GlobalMinimizer(1),GlobalMinimizer(2) )
    fprintf('Global Minimizer Value : %d', sym(globalMinimizerValue) ) %cast to symbolic
%     fprintf('Global Minimizer Value : %d', globalMinimizerValue )
end


% --- determine the angle of the Minimizer
% a1 = Minimizer(1,1)
% a2 = Minimizer(2,2)
a1 = double(Minimizer(1,1))
a2 = double(Minimizer(2,2))

% compute the angle <(e,e_1) where Minimizer = kappa* (e (x) e)
e = [sqrt((a1/(a1+a2))), sqrt((a2/(a1+a2)))];    % always positive under sqrt here .. basically takes absolute value here
angle = atan2(e(2), e(1)); 

% compute curvature kappa
kappa = (a1 + a2)

sqrt(a1*a2)
double(Minimizer)





% --- determine the TYPE of Minimizer by using 
% the number of solutions (Uniqueness?)
% the angle (axial- or non-axial Minimizer)


% Check Uniqueness -- Options: unique/twoMinimizers/1-ParameterFamily
if MinimizerCount == 1
    fprintf('Unique Minimzier')
    % Check if Minimizer is axial or non-axial:
    if (abs(angle-pi/2) < 1e-9 || abs(angle) < 1e-9) % axial Minimizer
     Type = 3;
    else % non-axial Minimizer   
     Type = 1;
    end
elseif MinimizerCount == 2
    fprintf('Two Minimziers')
    % Check if Minimizer is axial or non-axial:
    if (abs(angle-pi/2) < 1e-9 || abs(angle) < 1e-9) % axial Minimizer
        type = 3;
    else % non-axial Minimizer   
        fprintf('ERROR: Two non-axial Minimizers cannot happen!')
    end
else
    fprintf('1-Parameter family of Minimziers')
    % Check if Minimizer is axial or non-axial:
    if (abs(angle-pi/2) < 1e-9 || abs(angle) < 1e-9) % axial Minimizer
        fprintf('ERROR: axial Minimizers cannot happen for 1-Parameter Family!')
    else % non-axial Minimizer   
        Type = 2;
    end
end
% ------------------------------------------------------------------------------------------------------


fprintf("Angle = %d", angle)
fprintf("Curvature = %d", kappa)
fprintf("Type = %i", Type)

if make_FunctionPlot  
    fsurf(@(x,y) f_plus(x,y,q1,q2,q3,b1,b2,b3)) % Plot functions
    hold on
    plot3(A_plus,B_plus,T_plus,'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')
end


% Write symbolic solution to txt-File in Latex format
% fileID = fopen('txt.txt','w');
% fprintf(fileID,'%s' , latex(S_plus.v1));
% fclose(fileID);

%
%
% Compare with 'ClassifyMin-Code'

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

    Minimizer = sign*(GlobalMinimizer'*GlobalMinimizer) %sign correct?   should do this with symbolic Values! TODO

    % GlobalMinimizer'*GlobalMinimizer
    
    % Output as double
    sym(double(Minimizer))

    %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);
    % %Achtung veraltet!
    % % 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)

end

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

%
% TEST Min Function
%

clear all
clc
A = ones(10,1)
A(5) = 0
A(8) = 0
A(10) = 0

% A = magic(10)


[Min,MinIdx] = min(A, [], 'all', 'linear' )

% find(A == Min)

MinIndices = find(A(:) == min(A(:)))                  % Find All Minima

% [Min_minus,MinIdx_minus] = min(T_minus, [], 'all', 'linear')

numMinSP = size(MinIndices,1)



A = magic(5)
B = magic(5)

A-B

norm(A-B) < eps


%