%FETR  TRiangular Finite Elements
% [TYH,RH,ZH] = FETR(L) returns TYH the values of triangular FE at point
% (L.RY,L.ZY) and (RH,ZH) the position of the inner nodes. The vertical
% dimension is divided in L.P.NELEM/2+1 equidistant levels between
% ZLP,ZUP. Inner nodes: at the top and bottom level, one node at
% mid distance within the limiter (L.G.RL,L.G.ZL); for the other levels,
% two inner nodes equally distributed within the limiter. Edge limiter
% nodes: one on each side of all the levels plus one above and below the
% top and bottom inner node.
%    L
% L  I  L
% L I I L
% .......
% L I I L
% L  I  L
%    L
%
%
% For details, see: [MEQ-redbook] 
%
% [+MEQ MatlabEQuilibrium Toolbox+] Swiss Plasma Center EPFL Lausanne 2022. All rights reserved.
function [Tyh,rh,zh] = fetr(L)
 
 %% Nodes
 %    T
 % 0  1  3
 % 0 1 2 3
 % .......
 % 0 1 2 3
 % 0  1  3
 %    B
 rl = L.G.rl([1:end 1]);
 zl = L.G.zl([1:end 1]);
 nz = L.P.nelem/2+1;
 rip = L.bh(1);zlp = L.bh(2);
 rop = L.bh(3);zup = L.bh(4);
 r1 = repmat((rip+rop)/2,1,nz);
 z1 = linspace(zlp,zup,nz+2); z1 = z1(2:end-1);
 [r0,z0] = onlim(r1,(rip-rop)/2,z1,rl,zl);
 [r3,z3] = onlim(r1,(rop-rip)/2,z1,rl,zl);
 f = repmat(1/3,1,nz); f([1 end]) = 1/2;
 r1 = r0 + f.*(r3-r0);
 f = repmat(2/3,1,nz-2);
 k = 2:nz-1;
 r2 = r0(k) + f.*(r3(k)-r0(k));
 z2 = z1(k);
 [zb,rb] = onlim(z1(1),zlp-z1(1),r1(1),zl,rl);
 [zt,rt] = onlim(z1(end),zup-z1(end),r1(end),zl,rl);
 rh = [r1 r2];
 zh = [z1 z2];
 
 %% Triangles
 x0 = r0+1i*z0;
 x1 = r1+1i*z1;
 x2 = r2+1i*z2;
 x3 = r3+1i*z3;
 xb = rb+1i*zb;
 xt = rt+1i*zt;
 tr = [
  x1(1) xb    x0(1)
  x1(1) x3(1) xb
  x1(2) x0(2) x0(1)
  x1(2) x0(1) x1(1)
  x1(2) x1(1) x2(1)
  x2(1) x1(1) x3(1)
  x2(1) x3(1) x3(2)
  x1(end) xt      x0(end)
  x1(end)   x3(end) xt
  x1(end-1) x0(end-1) x0(end)
  x1(end-1) x0(end) x1(end)
  x1(end-1) x1(end) x2(end)
  x2(end)   x1(end) x3(end)
  x2(end) x3(end) x3(end-1)
  ];
 for k = 2:nz-2
  tr = [tr ; [
   x1(k+1) x0(k+1) x0(k)
   x1(k+1) x1(k)   x0(k)
   x2(k) x1(k+1) x1(k)
   x2(k) x1(k)   x2(k-1)
   x2(k) x2(k-1)   x3(k+1)
   x2(k-1)   x3(k)   x3(k+1)
   ]];
 end
 
 %% FE
 Tyh = reshape(cat(3,fev(tr,x1,L.ry,L.zy),fev(tr,x2,L.ry,L.zy)),[],2*nz-2);
 
end

function [x,y] = onlim(x,dx,y,xl,yl)
 f = zeros(size(x));
 for df = 2.^(-1:-1:-8)
  k = inpolygon(x+(f+df)*dx,y,xl,yl);
  f(k) = f(k)+df;
 end
 x = x+f*dx;
end

function t = fev(tr,x,ry,zy)
 t = ones(length(zy),length(ry),length(x));
 for k = 1:length(x)
  [i,j] = find(tr == x(k));
  for l = 1:length(j)
   b = zeros(1,3); b(j(l)) = 1;
   a = [real(tr(i(l),:)) ; imag(tr(i(l),:)) ; 1 1 1];
   c = b/a;
   f = min(max(c(1)*ry' + c(2)*zy + c(3),0),1);
   t(:,:,k) = min(t(:,:,k),f);
  end
 end
end

