function x = LagrangeFEM(n,p)
%       n = number of finite elements
%       k = degree of piecewise polynomials

% A function solving the problem 
%           -(k(x) u')' + q(x)u = f(x)   in (0,l)
%           u(0) = lbc, u(l) = rbc
% In particular, this function can be used to solve your
%    first/second programming assigment. (This is actually 
%    it's only purpose - see the notes below.)
  

%NOTE: I have written a "C-style" code ("for"-cycles and such,
%      instead of MatLab's built in vector operations), so 
%      that those of you who use C,C++ or Java can (almost)
%      copy it.

%NOTE: This is a slightly better code than the previous example,
%      although it still is by no means a "good" FEM code. 

% Coefficient definitions (the functions k,q and f are defined
%  at the end of the file.

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

  l=50; 
  lbc=0; rbc=0;
  
  quadrature_length = 7;

  % Evaluate the element size.
  h=l/n; 
  total_dof = p*n+1;
  

  % Populating the matrices and the right hand side.
  A1=zeros(total_dof,total_dof);
  A0=zeros(total_dof,total_dof); 
  B =zeros(total_dof,1);
  for i=1:n
      [wval,tval] = quadrature((i-1)*h,i*h);
      % the basis functions supported on element i : [(i-1)*p+1,(i-1)*p+p+1] 
      for q_ind = 1:quadrature_length
           % note that the "quadrature" function is mapping
           % reference quadrature points to points in the 
           % element, while here we map those points back to
           % evaluate the basis functions. This is obviously
           % redundant, and I only wrote it this way for clarity.
           x = tval(q_ind); X = (x - (i-1)*h)/h;

          for b1_ind=(i-1)*p+1:(i-1)*p+p+1
              phi_1val = basis_value(b1_ind-(i-1)*p,X);
              phi_1der = basis_prime(b1_ind-(i-1)*p,X)/h;

              for b2_ind=(i-1)*p+1:(i-1)*p+p+1
                  phi_2val = basis_value(b2_ind-(i-1)*p,X);
                  phi_2der = basis_prime(b2_ind-(i-1)*p,X)/h;
                  
                  % contribution to stiffness matrix
                  A1(b1_ind,b2_ind) = A1(b1_ind,b2_ind) + wval(q_ind)*k(x)*phi_1der*phi_2der;
                  
                  % and to mass matrix;
                  A0(b1_ind,b2_ind) = A0(b1_ind,b2_ind) + wval(q_ind)*q(x)*phi_1val*phi_2val;
              end
              
              B(b1_ind) = B(b1_ind) + wval(q_ind) * f(x) * phi_1val;
          end
      end
  end      


  % The global matrix is now:
  A=A0+A1;
  
  % Imposing the boundary conditions.
  %   Left,
  B(1)=lbc; A(1,1)=1;
  for i=2:total_dof
      A(1,i)=0;
      B(i)=B(i)-A(i,1)*B(1);
      A(i,1)=0;
  end
  %   Right.
  B(total_dof)=rbc;  A(total_dof,total_dof)=1;
  for i=1:total_dof-1
      A(total_dof,i)=0;
      B(i)=B(i)-A(i,total_dof)*B(total_dof);
      A(i,total_dof)=0;
  end

  % Solving,
  x=A\B

  % and plotting the solution against a uniform mesh.
  %   we use 500 points per element
  input = 0:l/(total_dof-1)/500:l;
  plot(input,exact_sol(input,x), input, w(input))
  
  discrete_max = max(abs(exact_sol(input,x)-w(input)))

  
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

  function y = basis_value(b,X)
      % see http://en.wikipedia.org/wiki/Lagrange_polynomials
      points = 0:1/p:1;
      y = 1;
      for j=1:p+1
          if j~=b
              y = y * (X-points(j)) / (points(b) - points(j));
          end
      end
  end

  function y = basis_prime(b,X)
      % see http://en.wikipedia.org/wiki/Lagrange_polynomials
      points = 0:1/p:1;
      y = 0;
      for j=1:p+1
          if j~=b
              v = 1;
              for j1=1:p+1
                  if ((j1~=b) && (j1~=j))
                      v = v * (X-points(j1)) / (points(b) - points(j1));
                  end
              end
              y = y + v / (points(b) - points(j));
          end
      end
  end

  function [w,t] = quadrature(a,b)
      % see http://en.wikipedia.org/wiki/Gaussian_quadrature for example.
      to = [0.949107912342759, -0.949107912342759, 0.741531185599394, -0.741531185599394, 0.405845151377397, -0.405845151377397, 0.000000000000000];
      wo = [0.129484966168870,  0.129484966168870, 0.279705391489277,  0.279705391489277, 0.381830050505119,  0.381830050505119, 0.417959183673469];
      w = (b-a)/2 * wo;
      t = (b-a)/2 * to + (a+b)/2;
  end
 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

  % The variable coefficents of the problem we're solving.
  function y = k(x)
      y = 1;
  end
  
  function y = q(x)
      y = 100/(8.8*10^7);
  end
  
  function y = f(x)
    y=200/(2*8.8*10^7)*x*(l-x);
  end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

  % The solution (added for computing the errors)
  function y = w(x)
      a = 100/(8.8*10^7)*l^2;
      b = 200/(2*8.8*10^7)*l^4;
      t = x/l;
      
      y = b/a * (-t.^2+t-2/a+2/(a*sinh(sqrt(a)))*(sinh(sqrt(a)*t)+sinh(sqrt(a)*(1-t))));
  end

  % (Approximate) Derivative of the solution (added for computing the errors)
  function y = w_prime(x)
    delta = 0.001;
    y = (w(x+delta)-w(x-delta))/(2*delta);
  end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

  % Computes the exact solution at an arbitrary point
  %   x_vector - point (or vector of points) where we want the solution
  %   dof - the computed values at the degrees of freedom
  function y = exact_sol(x_vector, dof)
      y = zeros(length(x_vector),1)';
      for i = 1 : length(x_vector)
          x_point = x_vector(i);
          element = floor(x_point/h)+1;
          if (element==n+1) element=n; end
          X_point = (x_point - (element-1)*h)/h;
          for base_ind=(element-1)*p+1:(element-1)*p+p+1
              y(i) = y(i) + dof(base_ind)*basis_value(base_ind-(element-1)*p,X_point);
          end
      end
  end
end