%%
clear all
close all
clc

elm_nodes=4;
bdf='C:\Users\Nicco\Documents\tesi\Codici\Al_Beam_Mesh_C2_90_deg.bdf';

str_coord='ijk';

[nEle,nNode,Cne,index_quad,index_tria,t,Pnn,gcoord_index,thk,Off,bc,Cbar,...
                    Off_beam,Pbarl,Pbarl_index,mat_e,gcoord,mcoord]=iFem_pre_v2(bdf,str_coord);
% Questa funzione importa il bdf, Cne è connectivity, Pnn è coordinate nodi
% mcoord importante, sono 60 matrici, una per ogni elemento, con versore di
% asse locale per ogni elemento, per righe. Serve poi in Rot a trasformare coordinate
% nodi da locale a globale.
q = zeros(nNode*6,1);% vettore dei gradi di libertà nodali, riferimento globale, 6 DOFs per ogni nodo (84 nodi)
A = load('C:\Users\Nicco\Documents\tesi\Codici\Disp.rpt');
B = load('C:\Users\Nicco\Documents\tesi\Codici\Rot.rpt');
q(1:6:end)=A(:,2); % In q ho 6 DOFs nodo 1, 6 DOFs nodo 2, ecc
q(2:6:end)=A(:,3);
q(3:6:end)=A(:,4);
q(4:6:end)=B(:,2);
q(5:6:end)=B(:,3);
q(6:6:end)=B(:,4);
straing = zeros(nEle*6*9,1);
strainconzeri = zeros(nEle*6*5,1);
matrice = zeros(nEle*6*9,nNode*6);
Rotespansa = zeros(24,24);
%qtot=zeros(6*4*nEle,1);
%qvero=zeros(nNode*6,1);
 for e=1:nEle
    %DEFINING THE GLOBAL COORDINATES OF THE NODES AND THE ROTATION MATIX
    % FOR EACH ELEMENT
    glocoord = Pnn(gcoord_index(Cne(e,2:elm_nodes+1)),:);
    Rot = mcoord(:,:,e);
    % Rot per trasformare coordinate
    
    % DEFINING THE TOP (Z2) AND BOTTOM (Z1) SURFACES OF THE ELEMENT
    z2=t(e)+Off(e);
    z1=-t(e)+Off(e);

    % TRANSFORMING THE GLOBAL COORDINATES TO THE LOCAL ONES
    locCoord = glocoord * Rot';

    X = locCoord(:,1);
    Y = locCoord(:,2);

    [Bm(:,:,e),Bk(:,:,e),Bg(:,:,e)]=compute_B_quad(X,Y,2,3,0,0);
    % Derivate funzioni di forma nei punti di Gauss (membranali, flessione,
    % taglio che non mi servono)
    % X e Y coordinate dei nodi nel sistema locale, 2 è flag per dire come
    % lavorare, ovvero calcola nei punti di Gauss, 3 punti di Gauss, che in
    % realtà è 3*3, 0 e 0 per flag = 1.
    for i=1:8
    Rotespansa((i-1)*3+1:(i-1)*3+3,(i-1)*3+1:(i-1)*3+3) = Rot;
    % E' una matrice 24*24 con 8 blocchi 3*3 in diagonale che hanno il
    % vettore di orientamento del SR
    end
    nodie = Cne(e,2:5); % Per ogni elemento salvo i 4 nodi della connectivity, corrispondenti alle colonne della matrice. 
    matricettasu = Bm(:,:,e)+z2*Bk(:,:,e); %Per ogni elemento salvo la matrice che deve moltiplicare i DOFs
    matricettagiu = Bm(:,:,e)+z1*Bk(:,:,e);%stessa cosa ma a -spessore
    matricettasu  = matricettasu*Rotespansa;
    matricettagiu  = matricettagiu*Rotespansa;
    k = 1;
    while k < 5 % Ovvero 4 nodi per ogni elemento
    % matrice salva per ogni 6 colonne i gradi di libertà di un nodo,
    % Per esempio, il nodo 7 avrà 6 DOFs dalle colonne 37 a 42 di
    % matrice, corrispondenti alle righe da 37 a 42 di q. 
    matrice(1+(e-1)*54:1+(e-1)*54+26,1+(nodie(k)-1)*6:1+(nodie(k)-1)*6+5) = matricettasu(:,1+(k-1)*6:1+(k-1)*6+5);
    matrice(1+(e-1)*54+27:1+(e-1)*54+53,1+(nodie(k)-1)*6:1+(nodie(k)-1)*6+5) = matricettagiu(:,1+(k-1)*6:1+(k-1)*6+5);
    k=k+1;
    end

% In questo momento le epsilon sono (9*3=27) per ogni punto di Gauss in un
% elemento e *2 (top e bottom), quindi 54 per ogni elemento. Il vettore
% qtot ha (6*4=24) ovvero 6DOFs per nodo per ogni elemento (*nEle). Quindi
% la matrice sarà una (54*24) espansa per nEle. 

end
straing = matrice*q;
% straing ha primi 9 elementi= epsxx(1 elemento) nei 9 punti di Gauss,
% secondi 9 elementi = epsyy(1 elemento) ecc... 
% Sono 60(nEle) pacchetti da (9*3*2)=54, ogni pacchetto ha prima 27 top poi
% 27 bottom per ogni elemento

% Ora devo trovare gli strain nei nodi dai punti di Gauss
 %Scrivo le funzioni di forma per l'elemento interno che ha come nodi i
 %punti di Gauss
% coordinate elemento nel SR locale
 %zie = [-1 1 0 1 -1];
 %etae = [-1 -1 0 1 1];
 % trasformazione coordinate elemento di Gauss
 %zig = sqrt(3)*zie;
 %etag = sqrt(3)*etae;
 % Voglio gli indici dei nodi di Gauss secondo la numerazione che corrispon
 % dono ai nodi dell'elemento, in senso antiorario

% Gli strain nei centroidi sono il quinto elemento ogni 9 di straing
% Così ottengo straincentoidi(1:6) = 3 epsxx,yy,xy top e 3 bottom per il
% primo elemento, e così via
straincentroidi = zeros(6*nEle,1);
for e=1:6*nEle
straincentroidi(e) = straing(9*(e-1)+5);
end
% Voglio sotto forma di matrice gli strain nei centroidi, voglio 3 colonne
% xx,yy,xy e tutti gli elementi da 1:nEle ripetuti due volte, prima top e
% poi bottom
straincentroidimat = zeros(nEle*2,3);
for e=1:nEle
straincentroidimat(e,:) = straincentroidi(6*(e-1)+1:6*(e-1)+3);
straincentroidimat(nEle+e,:) = straincentroidi(6*(e-1)+4:6*(e-1)+6);
end

%Carico gli strains nei centroidi
elements = [1:60];
straincentroidimatabq = load('C:\Users\Nicco\Documents\tesi\Codici\Strain_abaqus_centroide.rpt');
%%
erryy = zeros(nEle,1);
erryy = abs(straincentroidimat(1:nEle,2)-straincentroidimatabq(1:end,3))./abs(straincentroidimatabq(1:end,3));
errxx = zeros(nEle,1);
errxx = abs(straincentroidimat(1:nEle,1)-straincentroidimatabq(1:end,5))./abs(straincentroidimatabq(1:end,5));
errxy = zeros(nEle,1);
errxy = abs(-straincentroidimat(1:nEle,3)-straincentroidimatabq(1:end,7))./abs(straincentroidimatabq(1:end,7));
figure(1)
plot(elements,errxx,'-or')
xlabel('Elements')
ylabel('Error along X-axis')
figure(2)
plot(elements,erryy,'-or')
xlabel('Elements')
ylabel('Error along Y-axis')
figure(3)
plot(elements,errxy,'-or')
xlabel('Elements')
ylabel('Error in XY-plane')





% Matrice N è una 4*9, 4 nodi ottenuti da 9 Gauss
% Le coordinate dei nodi nel SR dei punti di Gauss sono sqrt(5/3), più grandi
% di 1 poiché 1 sono i punti di Gauss. 
chin = [-sqrt(5/3) sqrt(5/3) sqrt(5/3) -sqrt(5/3)];
etan = [-sqrt(5/3) -sqrt(5/3) sqrt(5/3) sqrt(5/3)];
% Ci sono 9 funzioni di forma, N1 deve moltiplicare gli strain del primo
% punto di Gauss e così via, mettendo nelle 4 righe le coordinate dei 4
% nodi nel SR di Gauss
for i = 1:4
 N(i,1) = 1/4*(chin(i)-1)*(etan(i)-1)*chin(i)*etan(i);
 N(i,7) = 1/4*(1+chin(i))*(etan(i)-1)*chin(i)*etan(i);
 N(i,9) = 1/4*(1+chin(i))*(1+etan(i))*chin(i)*etan(i);
 N(i,3) = 1/4*(chin(i)-1)*(1+etan(i))*chin(i)*etan(i);
 N(i,4) = 1/2*(1-chin(i)^2)*(etan(i)-1)*etan(i);
 N(i,6) = 1/2*(1-chin(i)^2)*(etan(i)+1)*etan(i);
 N(i,2) = 1/2*(chin(i)-1)*(1-etan(i)^2)*chin(i);
 N(i,8) = 1/2*(chin(i)+1)*(1-etan(i)^2)*chin(i);
 N(i,5) = (1-chin(i)^2)*(1-etan(i)^2);
end

%straingverticiecen = zeros(nEle*5*3*2,1); %creo un vettore con i 3 strain sopra e sotto ai vertici del quad e al centro (5*3*2)
%for e=1:nEle %per ogni elemento
    %for j=1:6 %per ogni strain 
    %straingverticiecen((e-1)*30+5*(j-1)+1:(e-1)*30+5*j) = straing((e-1)*54+9*(j-1)+1:2:(e-1)*54+9*j);
    %end
%end
%Prendo ogni 2 posizioni per ogni 9 punti di Gauss, così prendo il
%i 4 nodi(1,3,7,9) e il centroide (5). 
%Questo vettore straingaussverticiecentroide è fatto 30 posizioni per un elemento, 3*2 strain per ogni
%nodo e per il centroide (3*2*5), ho primi 6 per primo elemento e via così
% Voglio togliere gli zeri dei centroidi
%straingvertici = zeros(nEle*6*4,1);
%straingvertici(1:2) = straingverticiecen(1:2);
%straingvertici(end-1:end) = straingverticiecen(end-1:end);
%for k=0:6*nEle-2
    %straingvertici(3+4*k:6+4*k) = straingverticiecen(4+5*k:7+5*k);
%end

%Ora voglio gli strain nei nodi
strainnodalixxtop = zeros(nEle*4,1);
strainnodaliyytop = zeros(nEle*4,1);
strainnodalixytop = zeros(nEle*4,1);
strainnodalixxbottom = zeros(nEle*4,1);
strainnodaliyybottom = zeros(nEle*4,1);
strainnodalixybottom = zeros(nEle*4,1);
for e=1:nEle
    %for j=1:6
 %strainnodali((e-1)*24+4*(j-1)+1:(e-1)*24+4*j) = N*straingvertici((e-1)*24+4*(j-1)+1:(e-1)*24+4*j); 
    strainnodalixxtop((e-1)*4+1:(e-1)*4+4) = N*straing((e-1)*9*6+1:(e-1)*9*6+9); 
    strainnodaliyytop((e-1)*4+1:(e-1)*4+4) = N*straing((e-1)*9*6+10:(e-1)*9*6+10+8);
    strainnodalixytop((e-1)*4+1:(e-1)*4+4) = N*straing((e-1)*9*6+19:(e-1)*9*6+19+8);
    strainnodalixxbottom((e-1)*4+1:(e-1)*4+4) = N*straing((e-1)*9*6+28:(e-1)*9*6+28+8);
    strainnodaliyybottom((e-1)*4+1:(e-1)*4+4) = N*straing((e-1)*9*6+37:(e-1)*9*6+37+8);
    strainnodalixybottom((e-1)*4+1:(e-1)*4+4) = N*straing((e-1)*9*6+46:(e-1)*9*6+46+8);
    %end
end
% Sfrutto il fatto che straing ha 54 strain per elemento, 9 xx top, 9 yy
% top e così via. Ogni pacchetto di 9 strain di Gauss ottengo 4 strain
% nodali. Avrò strainnodalixxtop per esempio che avrà 4 epsxxtop nei 4
% nodi del 1 elemento, poi 4 epxxtop nei 4 nodi del secondo elemento 
% e così via. 4nodi*60 elementi, avrò 6 vettori, uno per ogni eps.
strainnodali = zeros(nEle*4*6,1);
for e=1:nEle
    strainnodali((e-1)*24+1:(e-1)*24+4)=strainnodalixxtop((e-1)*4+1:(e-1)*4+4);
    strainnodali((e-1)*24+5:(e-1)*24+8)=strainnodaliyytop((e-1)*4+1:(e-1)*4+4);
    strainnodali((e-1)*24+9:(e-1)*24+12)=strainnodalixytop((e-1)*4+1:(e-1)*4+4);
    strainnodali((e-1)*24+13:(e-1)*24+16)=strainnodalixxbottom((e-1)*4+1:(e-1)*4+4);
    strainnodali((e-1)*24+17:(e-1)*24+20)=strainnodaliyybottom((e-1)*4+1:(e-1)*4+4);
    strainnodali((e-1)*24+21:(e-1)*24+24)=strainnodalixybottom((e-1)*4+1:(e-1)*4+4);
end
% Qui metto i 6 vettori in fila
% ! Qui sono fatti tali che ho 4 epsxx per i 4 nodi del primo elemento, poi 4
% epsyy e così via, 3 per ogni nodo sopra e sotto, 4*6=24 per ogni elemento.
% Quindi i primi 24 componenti sono xxtop, yytop, xytop, xxbottom ecc per
% il primo elemento, e via così per il secondo
strainnodalimat = zeros(nEle*4,6);
  for e=1:nEle
          for k=1:6
strainnodalimat((e-1)*4+1:(e-1)*4+4,k) = strainnodali((e-1)*24+(k-1)*4+1:(e-1)*24+(k-1)*4+4);
    end
  end
  %  Ho creato qui una matrice che ha sulle righe 4 nodi di ogni elemento
  %  (4*nEle) e sulle colonne le 3 eps top e le 3 eps bottom (3*2)
  % L'ordine dei 4 nodi di ogni pacchetto dovrebbe essere quello antiorario
  % della connectivity
l=1;
i = zeros(nNode,1);
  strainmediati = zeros(nNode,6);
for e=1:nEle
    for k=2:5
strainmediati(Cne(e,k),:)=strainmediati(Cne(e,k),:)+strainnodalimat(l,:); % se non è antiorario per Gauss è un casino 
i(Cne(e,k))=i(Cne(e,k))+1; %contatore aumenta ogni volta che sommo in un nodo
l = l+1;
    end
end
% Io ho considerato che procedendo per ogni elemento si vada in senso
% antiorario sia per la connectivity sia per i punti di Gauss, ma non so se
% sia giusto. Così facendo, io avanzo nella matrice con i nodi e so già che troverò i nodi della
% connectivity 
for l=1:nNode
strainmediati(l,:)=strainmediati(l,:)/i(l); %media per ogni nodo (ogni riga)
end



 
% Bm ha 60 matrici 27*24, poiché 3 epsilon per 9 punti di Gauss (3*9=27).
% Prime 9 righe sono punti di Gauss epsilonxx, seconde 9 epsyy, ultimi 9
% epsxy. 24 perché 4 nodi*6 DOFs. 60 sono gli elementi.
% Devo ottenere grandezze=FDF*DOFs. 
% Prima devo trasformare DOFs globali in locali, poi li posso moltiplicare
% per FDF derivate. 
% Occhio a z perché membranale + z*flessionale, c'è t che è spessore. 
% Report Disp sono spostamenti e Rot sono rotazioni. 
% Nella matrice (?) si hanno 6 DOFs del primo nodo, poi 6 per secondo e così
% via. 
% Mi ricavo DOFs nodali e posso confrontare con Patran, dato che ho il bdf.
% Sono due carichi concentrati negli estremi. Lui utilizza ijk come sistema
% di riferimento, x da nodo 1 a nodo 2. 
% Attenzione che con queste FDF trovo strain nei punti di Gauss, uno sarà
% il centroide, io faccio confronto con centroide. Centroide dovrebbe
% essere il quinto. In quella matrice il 5, 5+9, ecc sarà il centroide. Poi
% così posso confrontare con Patran.
% Perché ho due modelli identici? Perché ijk in uno è a 90° e nell'altro
% inclinato di 90°. x diventa y e y diventa -x. Si vede su mcoord. Devo
% ottenere quelle che erano y uguali a -x e resto non cambia. 
% Prossimo step è di calcolare deformazioni nei nodi a partire da Gauss. 
% Useremo interpolazione biquadratica, usiamo FDF come se fossero un
% elemento finito. Siccome codice è iFEM, queste FDF sono quelle che già
% calcoliamo per calcolare gli spostamenti. 
% Ora lavoro per elementi, calcolare DOFs nodali dell'elemento e calcolare
% gli strain, una cosa ottima sarebbe creare vettore DOFs q chge sarà (84*6)*1,
% mentre matrice (6*9*60)*(84*6), 6 poiché 3 top e 3 bottom = 6. 
