clear all
clc

elm_nodes=4;
bdf='Al_Beam_Mesh_C2.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('Disp.rpt');
B = load('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);
%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);
    z1=-t(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.
    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
    k = 1;
    while k < 5 % Ovvero 4 nodi, ho immaginato che le 24 colonne di Bm e Bk siano 6 dofs per il primo nodo della
        % connectivity, 6 dofs per il secondo e via così fino al quarto
    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
 N1 = @(zig,etag) 1/4*(1-zig)*(1-etag);
 N2 = @(zig,etag) 1/4*(1+zig)*(1-etag);
 N3 = @(zig,etag) 1/4*(1+zig)*(1+etag);
 N4 = @(zig,etag) 1/4*(1-zig)*(1+etag);
% 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;
 % 4*4 FDF, 4 per ogni nodo di un elemento
 N11 = N1(zig(1),etag(1));
 N21 = N2(zig(1),etag(1));
 N31 = N3(zig(1),etag(1));
 N41 = N4(zig(1),etag(1));
 N12 = N1(zig(2),etag(2));
 N22 = N2(zig(2),etag(2));
 N32 = N3(zig(2),etag(2));
 N42 = N4(zig(2),etag(2));
 N13 = N1(zig(3),etag(3));
 N23 = N2(zig(3),etag(3));
 N33 = N3(zig(3),etag(3));
 N43 = N4(zig(3),etag(3));
 N14 = N1(zig(4),etag(4));
 N24 = N2(zig(4),etag(4));
 N34 = N3(zig(4),etag(4));
 N44 = N4(zig(4),etag(4));
 
 N = [N11 N21  N31 N41; % N è una 4*4 
     N12 N22  N32 N42;
     N13 N23  N33 N43;
     N14 N24  N34 N44];

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
strainnodali = zeros(nEle*6*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); 
    end
end
% Ora ho 6 strain in ogni nodo per nEle elementi, ma i nodi si ripetono!
% strainnodali ha per ogni elemento i 3*2 strain nei 4 nodi, totale quindi
% è: 3*2*4*nEle(60) = 1440, devo capire comunque come mediare i nodi che
% sono in comune tra più elementi.
% ! Qui sono fatti tali che ho 4 epsxx per i 4 nodi del primo elemento, poi 4
% epsyy e così via, 4*6=24 per ogni elemento.
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=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. 