function [ PE ] = PEBeam( w )
%solveBeam.M calculates potential energy for given deflection
% For ME256 3rd Homework
disp('This function is dependent on input files : node.dat, elem.dat, dispbc.dat, forces.dat and w')

load node.dat
load elem.dat
load forces.dat
load dispbc.dat
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% PRE-PROCESSING
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Identify the number of nodes, X and Y Coordinates of the nodes
NNODE = size(node,1);
nx = node(:,2);
ny = node(:,3);
%
% Identify the number of elements and form an element connectivity array,
% the cross-section and Young's modulus arrays.
NELEM = size(elem,1);
ncon = elem(:,[2 3]);
beam_width = elem(:,4);
beam_depth = elem(:,5);
E = elem(:,6);
A = beam_width.*beam_depth;
Inertia = beam_width.*(beam_depth.^3)/12;
%
% Arrange force information into a force vector, F
F = zeros(3*NNODE,1);			% Initialization
Nforce = size(forces,1);
for i = 1:Nforce,
    dof = (forces(i,2)-1)*3 + forces(i,3);
    F(dof) = forces(i,4);
end
%
% Displacement boundary conditions
Nfix = size(dispbc,1);
j = 0;
for i = 1:Nfix,
    j = j + 1;
    dispID(j) = (dispbc(i,2)-1)*3+dispbc(i,3);
    dispVal(j) = dispbc(i,4);
end
[dispID sortIndex] = sort(dispID);
dispVal = dispVal(sortIndex);
%
% Compute the lengths of the elements
for ie=1:NELEM,
    eye = ncon(ie,1);
    jay = ncon(ie,2);
    L(ie) = sqrt ( (nx(jay) - nx(eye))^2 + (ny(jay) - ny(eye))^2 );
end

Ae=A; Le=L; Ee=E; Ine=Inertia;




K = zeros(3*NNODE,3*NNODE);				% Initialize global stiffness matrix
k = zeros(6,6);						% Initialize local stiffness matrix

for ie=1:NELEM,
  eye = ncon(ie,1);
  jay = ncon(ie,2);
  
% Form the transformation matrix, Lambda.
  L = Le(ie);
  A = Ae(ie);
  E = Ee(ie);
  In= Ine(ie);
  lox = (nx(jay)-nx(eye))/L; mox = (ny(jay)-ny(eye))/L;
  loy = -mox; moy = lox;
  Lambda = [ lox mox  0   0   0   0   ; ...
	        -mox lox  0   0   0   0   ; ...
              0   0   1   0   0   0   ; ...
              0   0   0  lox  mox 0   ; ...
              0   0   0 -mox  lox 0   ; ... 
              0   0   0   0   0   1  ];
% Form local element matrix
  k(1,1)=E*A/L;			k(1,4)=-k(1,1);	
  k(2,2)=12*E*In/(L^3);		k(2,3)=6*E*In/(L^2);
  k(2,5)=-k(2,2);		k(2,6)=k(2,3);
  k(3,2)=k(2,3);		k(3,3)=4*E*In/L;
  k(3,5)=-k(2,3);		k(3,6)=2*E*In/L;
  k(4,1)=-k(1,1);		k(4,4)=k(1,1);
  k(5,2)=-k(2,2);		k(5,3)=-k(2,3);
  k(5,5)=k(2,2);		k(5,6)=-k(2,3);
  k(6,2)=k(2,3);		k(6,3)=k(3,6);
  k(6,5)=-k(2,3);		k(6,6)=k(3,3);

  klocal = Lambda' * k * Lambda;


% Form ID matrix to assemble klocal into the global stiffness matrix, K.
  id1 = 3*(eye-1) + 1;
  id2 = id1 + 1;
  id3 = id2 + 1;
  id4 = 3*(jay-1) + 1;
  id5 = id4 + 1;
  id6 = id5 + 1;

  K(id1,id1) = K(id1,id1) + klocal(1,1);
  K(id1,id2) = K(id1,id2) + klocal(1,2);
  K(id1,id3) = K(id1,id3) + klocal(1,3);
  K(id1,id4) = K(id1,id4) + klocal(1,4);
  K(id1,id5) = K(id1,id5) + klocal(1,5);
  K(id1,id6) = K(id1,id6) + klocal(1,6);
  
  K(id2,id1) = K(id2,id1) + klocal(2,1);
  K(id2,id2) = K(id2,id2) + klocal(2,2);
  K(id2,id3) = K(id2,id3) + klocal(2,3);
  K(id2,id4) = K(id2,id4) + klocal(2,4);
  K(id2,id5) = K(id2,id5) + klocal(2,5);
  K(id2,id6) = K(id2,id6) + klocal(2,6);

  K(id3,id1) = K(id3,id1) + klocal(3,1);
  K(id3,id2) = K(id3,id2) + klocal(3,2);
  K(id3,id3) = K(id3,id3) + klocal(3,3);
  K(id3,id4) = K(id3,id4) + klocal(3,4);
  K(id3,id5) = K(id3,id5) + klocal(3,5);
  K(id3,id6) = K(id3,id6) + klocal(3,6);
  
  K(id4,id1) = K(id4,id1) + klocal(4,1);
  K(id4,id2) = K(id4,id2) + klocal(4,2);
  K(id4,id3) = K(id4,id3) + klocal(4,3);
  K(id4,id4) = K(id4,id4) + klocal(4,4);
  K(id4,id5) = K(id4,id5) + klocal(4,5);
  K(id4,id6) = K(id4,id6) + klocal(4,6);

  K(id5,id1) = K(id5,id1) + klocal(5,1);
  K(id5,id2) = K(id5,id2) + klocal(5,2);
  K(id5,id3) = K(id5,id3) + klocal(5,3);
  K(id5,id4) = K(id5,id4) + klocal(5,4);
  K(id5,id5) = K(id5,id5) + klocal(5,5);
  K(id5,id6) = K(id5,id6) + klocal(5,6);

  K(id6,id1) = K(id6,id1) + klocal(6,1);
  K(id6,id2) = K(id6,id2) + klocal(6,2);
  K(id6,id3) = K(id6,id3) + klocal(6,3);
  K(id6,id4) = K(id6,id4) + klocal(6,4);
  K(id6,id5) = K(id6,id5) + klocal(6,5);
  K(id6,id6) = K(id6,id6) + klocal(6,6);
end

% Store unlaltered K as Ksing before applying the boundary conditions.
  Ksing = K;
Fsing=F;
%det(K)
%inv(K);
%pause

% Imposing displacement boundary conditions
% ------------------------------------------
% dispID array contains the dof which are assigned zero values.
[sm,sn] = size(dispID);
Ndbc = sn;
for nd=1:Ndbc,
 K = matcut(K,dispID(nd)-nd+1);
 F = veccut(F,dispID(nd)-nd+1);
end

% To solve for unknown displacements.
U = inv(K)*F;
SE = .5*U'*K*U;
newU=zeros(NNODE*3,1);
newU(2:3:end)=w;
PE=.5*newU'*Ksing*newU-Fsing'*newU;
end
