clear all;
close all;
clc;
format long;
tic
%%--------------------------------Subroutine-SSS----------------------------
%%Meshing, and evaluating the COST for each grid in (q0,q1,q2) space---------

% Paremeters inputs for SSS
IP = fopen('input.txt','r');                                    % scan all data extracting variables
instring = fgets(IP);
E0 = sscanf(instring(1:10), '%e')                         % E0, monochromatic kinetic energy of ion (MeV) 

instring = fgets(IP);
Zmin = sscanf(instring(1:10), '%e')                    % Zmin, minimum boundary of desired depth region (¦Ìm)         

instring = fgets(IP);
Zmax = sscanf(instring(1:10), '%e')                   % Zmax, Maximum boundary of desired depth region (¦Ìm)

instring = fgets(IP);
Dam_desired = sscanf(instring(1:10), '%e')       % Dam_desired, the minimum desired damage in the same region to avoid the region is beyond the ion projected range

instring = fgets(IP);
dq0 = sscanf(instring(1:10), '%e')                      % dq0, dq0/180*pi for inclination angle, grid size definition for the DOFs in SSS 

instring = fgets(IP);
dq1 = sscanf(instring(1:10), '%e')                      %  dq1, (dq1, dq2)/180*pi for rotation axis

instring = fgets(IP);
dq2 = sscanf(instring(1:10), '%e')                      %  dq2,  Note: too small grid size is time-comsuming

instring = fgets(IP);
meshphi = sscanf(instring(1:10), '%e')               % meshphi, delta phi size when rotating the sample, meshphi>90 is recommended


meshphi ;                                                            % meshphi, delta phi size when rotating the sample, meshphi>90 is recommended 
dphi=2*pi/meshphi; 
phi=0:dphi:2*pi-dphi;
cosine=phi*0;                                                     %direction cosine definition

%%-------------------------------------------------------------------------------------------------
% --------Reading Excess and damage from NN-trained surrogate models---------------- 
                                                                            % ion beam energy: 5MeV
E = importdata('Outputs/total-excess.txt');
D= importdata('Outputs/total-damage.txt');

z = (0.02 : 0.005 : 2.10) ;                                     % Ion depth in NN-trained surrogate models 
Zmin;  Zmax;                                                       % Desired depth region definition
minZ=find(abs(z-Zmin)<=1e-4);                       % Find the positions of Region boundaries
maxZ=find(abs(z-Zmax)<=1e-4);
Zdel = 0.005;
Z = (Zmin : Zdel : Zmax);                                   % Desired depth region
        
%------------Grid meshing on the DOFs of SSS -----------------
%  q0, q1, q2----Inclination angle and rotation axis
fidcost = fopen('Outputs/cost.txt','w')    
dq0; dq1; dq2;                                                    % grid size for q0, q1, q2, too small grid size is time-consuming
Dam_desired;                                                     % Damage-desired definition 

%%
for i=0:dq0:89
    i
     q0 = i/90*(pi/2);  
     for j=0:dq1:89      
          q1 = j/90*(pi/2);    
           for k= 0:dq2:359
                q2 = k/360*(2*pi);  
    
                 pphi=phi;
                 cosine = sin(q0)*sin(q1)*[cos(q1)*sin(q2)*(1-cos(pphi))-cos(q2)*...
                                 sin(pphi)]+cos(q0)*[1-sin(q1)^2*(1-cos(pphi))]  ;
                 %%plot(phi,cosine); hold on;           % Only for debugging, commented out when running 
                 %---------------------------------------------------------------------------------
                 HH = 0;
                 sumHH =0;
                 sumDD =0;
                 sumDDZ=0;
                 sumEEZ=0;
                 aveDD = 0;
                 aveDDZ=0;
                 aveEE =0;
                 aveEEZ=0;

                 AA = 0;
                 cost1=0; cost2=0;      
                 %if(i~=j & j~=k & k~=i)
                      for nn=1:length(phi)
                            theta(nn) = acos(cosine(nn))*180/pi;             %theta is the directional cosine of ion momentum during SSS
                            theta(nn) = roundn(theta(nn),0);
                            if(theta(nn)<=89)              
                                    HH = heaviside(cosine(nn)) ;
                                    AA = AA + dphi* HH.* cosine(nn);         % Denominator of averaged quantity in SSS
                                    sumDD     = sumDD + dphi*HH.*cosine(nn)* D(:,theta(nn)+1)' ;  
                                    sumDDZ   = sumDDZ + dphi*HH.*cosine(nn)* D(minZ:maxZ,theta(nn)+1)' ; 
                                    sumHH     = sumHH + dphi*HH.*cosine(nn)* E(:,theta(nn)+1)' ;
                                    sumEEZ    = sumEEZ + dphi*HH.*cosine(nn)* E(minZ:maxZ,theta(nn)+1)';
                            end 
                            aveDD   =  sumDD/AA;  
                            aveDDZ =  sumDDZ/AA;
                            aveEE     =  sumHH/AA;  
                            aveEEZ   =  sumEEZ/AA;
                      end
                      % plot(z,aveHH,'b',Z,aveHHZ,'bo');hold on;          % Only for debugging, commented out when running the code 
                      for mm=1:length(Z)
                             cost1 = cost1+Zdel*aveEEZ(mm);                              %COST1 definition: averaged excess          
                             cost2 = cost2 + Zdel*aveEEZ(mm)*aveEEZ(mm);     %COST2 definition: mean-squared averaged excess
                      end
                      if(all(aveDDZ>Dam_desired))                                                                      % desired damage definition: > 8  
                               fprintf(fidcost,'%20.15f\t',i,j,k,q0,q1,q2,cost1,cost2);
                               fprintf(fidcost,'\n');
                      end
                      %-----------------------------------------------------
                 %end
           end
      end
end
fclose(fidcost);  
%
toc
fprintf('%s','COST Evaluation is done');

