%SHAPMEX shape parameters
%
% This is the MATLAB equivalent implementation of libmeq/shap.c.
% A more detailed help is available in SHAPMEX.
%
% [+MEQ MatlabEQuilibrium Toolbox+] Swiss Plasma Center EPFL Lausanne 2022. All rights reserved.

function  [rgeom,zgeom,aminor,epsilon,kappa,delta,deltal,deltau,...
  rrmax,zrmax,rrmin,zrmin,rzmax,zzmax,rzmin,zzmin]...
  = shapmexm(rq,zq,rB,zB,rA,zA,dr2FA,dz2FA,dr,dz,verbose)

 narginchk(10,11);
 if nargin < 11
  verbose=0;
 end
 %% shape parameters from contour points. For LCFS, add rB,zB to list of contour points
 nrq = size(rq,2);
 if ~((isempty(rB) && isempty(zB)) || (numel(rB)==1 && numel(zB)==1))
  error('rB,zB must be either both empty or scalar');
 end
 
 [noq,npq] = size(rq);
 
 [rrmin,rrmax,zrmin,zrmax,zzmin,zzmax,rzmin,rzmax] = deal(zeros(npq,1));
 [rrmin(:),irmin]=min(rq); zrmin(:) = zq(sub2ind([noq,npq],irmin,1:nrq));
 [rrmax(:),irmax]=max(rq); zrmax(:) = zq(sub2ind([noq,npq],irmax,1:nrq));
 
 [zzmin(:),izmin]=min(zq); rzmin(:) = rq(sub2ind([noq,npq],izmin,1:nrq));
 [zzmax(:),izmax]=max(zq); rzmax(:) = rq(sub2ind([noq,npq],izmax,1:nrq));
 
 %% refine estimate by fitting circle to three closest points
 for ipq=1:numel(rrmin)
  rrq=rq(:,ipq); zzq=zq(:,ipq);
  
  if ipq==npq % last surface may be bounded by rB, zB point instead
   rrB=rB; zzB=zB;
  else
   rrB=[]; zzB=[];
  end
  
  r1 = rrmin(ipq); z1 = zrmin(ipq);
  [rrmin(ipq),zrmin(ipq)] =  refine_estimate(r1,z1,rrq,zzq,rrB,zzB,dr,dz,-1,0);
  
  r1 = rrmax(ipq); z1 = zrmax(ipq);
  [rrmax(ipq),zrmax(ipq)] = refine_estimate(r1,z1,rrq,zzq,rrB,zzB,dr,dz,+1,0);
  
  r1 = rzmin(ipq); z1 = zzmin(ipq);
  [rzmin(ipq),zzmin(ipq)] = refine_estimate(r1,z1,rrq,zzq,rrB,zzB,dr,dz,0,-1);
  
  r1 = rzmax(ipq); z1 = zzmax(ipq);
  [rzmax(ipq),zzmax(ipq)] = refine_estimate(r1,z1,rrq,zzq,rrB,zzB,dr,dz,0,+1);
  
 end
 %%
 
 aminor  = [0;(rrmax-rrmin)/2]; % minor radius
 rgeom   = [rA;(rrmax+rrmin)/2];
 zgeom   = [zA;(zzmax+zzmin)/2];
 epsilon = aminor./rgeom; % inverse aspect ratio
 
 kappa0  = sqrt(dr2FA/dz2FA);
 
 ie = 1+(1:nrq);
 kappa   = [kappa0;(zzmax-zzmin)./(rrmax-rrmin)];
 deltal  = [0;(rgeom(ie)-rzmin)./aminor(ie)]; % upper triangularitiy
 deltau  = [0;(rgeom(ie)-rzmax)./aminor(ie)]; % lower triangularity
 delta   = (deltal+deltau)/2; % average triangularity
 
 rrmax = [rA;rrmax];
 rzmax = [rA;rzmax];
 rrmin = [rA;rrmin];
 rzmin = [rA;rzmin];
 zrmax = [zA;zrmax];
 zzmax = [zA;zzmax];
 zrmin = [zA;zrmin];
 zzmin = [zA;zzmin];
 %% debug
 if verbose
  hold on;
  plot(rrq,zzq,'.'); hold on;
  plot(rrmin,zrmin,'ks',rrmax,zrmax,'ks','markersize',15);
  plot(rzmin,zzmin,'ko',rzmax,zzmax,'ko','markersize',15);
  plot(rgeom,zgeom,'co');
  plot(rA,zA,'or');
  plot(rB,zB,'xr');
 end
 
end

function [r1,z1,r2,z2] = find_closest(rq,zq,r,z)
 
 dist=sum(([r,z] - [rq,zq]).^2,2);
 
 distmin1 = realmax;
 distmin2 = realmax;
 imin1 = 0;
 imin2 = 0;
 
 for ii=1:numel(dist)
  if dist(ii)==0
   continue
  end
  if dist(ii)<distmin2
   if dist(ii)<distmin1
    imin2=imin1; imin1=ii;
    distmin2=distmin1; distmin1=dist(ii);
   else
    imin2=ii;
    distmin2=dist(ii);
   end
  end
 end
 
 r1 = rq(imin1); z1 = zq(imin1);
 r2 = rq(imin2); z2 = zq(imin2);
 
end

function [rr,zz] = refine_estimate(r1,z1,rq,zq,rB,zB,dr,dz,drd,dzd)
 if ~isempty(rB) && ...
   (((drd*(rB-r1))>0 || (dzd*(zB-z1))>0) ...
   || (abs(rB-r1)<dr && abs(zB-z1)<dz))
  % rB,zB replaces this point
  rr = rB; zz = zB;
 else
  % refine the point by fitting a circle through 2 close points
  [r2,z2,r3,z3] = find_closest([rq;rB],[zq;zB],r1,z1);
  [rc,zc,dc,s] = fitcirc(r1,z1,r2,z2,r3,z3,0);
  assert(all(s),'fitcirc failed')
  rr = rc+drd*dc; zz=zc+dzd*dc;
 end
end

function [xcc,ycc,rcc,s] = fitcirc(xx1,yy1,xx2,yy2,xx3,yy3,verbose)
 % function [xc,yc,rc,s] = fitcirc(x1,y1,x2,y2,x3,y3)
 % Fits a circle to a set of 3 points
 % s=0 if ok, s=1 if no fit could be found
 
 
 %% Background
 % fit conditions, circle with center (a,b) and radius c:
 % (x1-a)^2 + (y1-b)^2 = c^2
 % (x2-a)^2 + (y2-b)^2 = c^2
 % (x3-a)^2 + (y3-b)^2 = c^2
 %
 % x1^2-2a*x1 + a^2 + y1^2-2b*y1+b^2 = c^2
 % x2^2-2a*x2 + a^2 + y2^2-2b*y2+b^2 = c^2
 % x3^2-2a*x3 + a^2 + y3^2-2b*y3+b^2 = c^2
 %
 % Sutract (2-1), (3-1), (3-2)
 % b*(y2-y1) + a*(x2-x1)  = ((x2^2-x1^2)+ y2^2-y1^2 )/2
 % b*(y3-y1) + a*(x3-x1)  = (((x3^2-x1^2)+ y3^2-y1)^2 )/2
 % b*(y3-y2) + a*(x3-x2)  = ((x3^2-x2^2)+ y3^2-y2)^2 )/2
 %
 % This is 3 equations with 2 unknowns. We choose which 2
 % equations checking that the problem is not singular
 %
 % Suppose you choose the first two equations, then:
 % d1x = x2-x1
 % d2x = x3-x1
 % d1y = y2-y1
 % d2y = y3-y1
 % e1 = ( x2^2-x1^2 + y2^2-y1^2 )/2
 % e2 = ( x3^2-x1^2 + y3^2-y1^2 )/2
 %
 % [d1x d1y
 %  d2x d2y] * [a;b] = [e1;e2]
 
 % Analytical solution:
 % a=(d2y*e1-d1y*e2)/(d1x*d2y-d1y*d2x)
 
 %%
 if nargin<7
  verbose = 0;
 end
 
 xcc = zeros(size(xx1));
 ycc = zeros(size(xx1));
 rcc = zeros(size(xx1));
 s = false(size(xx1));
 
 for ii=1:numel(xx1)
  x1 = xx1(ii); x2=xx2(ii); x3=xx3(ii);
  y1 = yy1(ii); y2=yy2(ii); y3=yy3(ii);
  
  % check for cases of vertical lines, don't use this combination for fit if it is so
  if x2~=x1
   d1x = x2-x1;
   d1y = y2-y1;
   e1 = (x2^2-x1^2+ y2.^2-y1.^2 )/2;
   if x3~=x2
    d2x = x3-x2;
    d2y = y3-y2;
    e2 = (x3.^2-x2.^2 + y3.^2-y2.^2)/2;
   else
    d2x = x3-x1;
    d2y = y3-y1;
    e2 = (x3.^2-x1.^2 + y3.^2-y1.^2)/2;
   end
  elseif (x3~=x2) && (x3~=x1)
   d1x = (x3-x2);
   d1y = (y3-y2);
   e1 = (x3^2-x2^2 + y3.^2-y2.^2)/2;
   
   d2x = (x3-x1);
   d2y = (y3-y1);
   e2 = (x3^2-x1^2 + y3.^2-y1.^2)/2;
  else
   warning('invalid set of points');
   xcc(ii)=x1; ycc(ii)=x1; rcc(ii)=0; s(ii)=false; continue;
  end
  
  % Numeric solution
  %   AA = [d1x,d1y;d2x,d2y]; bb = [e1;e2];
  %   xx = AA\bb;
  %   xc = xx(1); yc=xx(2);
  
  % Analytical solution
  det=(d1x*d2y-d1y*d2x);
  if det==0
   warning('points may not be colinear');
   xcc(ii)=x1; ycc(ii)=x1; rcc(ii)=0; s(ii)=false; continue;
  end
  
  iddet = 1./det;
  xc = (+d2y*e1-d1y*e2) * iddet;
  yc = (-d2x*e1+d1x*e2) * iddet;
  rc = sqrt( (x1-xc)^2 + (y1-yc)^2 );
  
  xcc(ii) = xc;
  ycc(ii) = yc;
  rcc(ii) = rc;
  s(ii)=true; % ok
  
  if verbose
   clf;
   plot([x1,x2,x3],[y1,y2,y3],'x'); hold on;
   thg = linspace(0,2*pi,101);
   plot(xc+rc*cos(thg),yc+rc*sin(thg))
   plot(xc,yc,'+r');
   drawnow
  end
 end
end
