/* [+MEQ MatlabEQuilibrium Toolbox+] Swiss Plasma Center EPFL Lausanne 2022. All rights reserved. */
# include "meq.h"
# ifdef SINGLE
# define FCT  sshap
# define REFINE srefine
# define FITCIRC sfitcirc
# define FIND_CLOSEST sfind_closest
# else
# define FCT  dshap
# define REFINE drefine
# define FITCIRC dfitcirc
# define FIND_CLOSEST dfind_closest
# endif


/* Sub-functions */
void REFINE(FLT* r1, FLT* z1, FLT* rq, FLT* zq, int noq, int isLCFS, FLT rB, FLT zB, FLT dr, FLT dz, FLT drd, FLT dzd);
void FITCIRC(FLT* xc, FLT* yc, FLT* rc, int* s,
        FLT x1, FLT y1, FLT x2, FLT y2, FLT x3, FLT y3);
void FIND_CLOSEST(FLT* r1, FLT* z1, FLT* r2, FLT* z2, FLT* rq, FLT* zq, int noq, FLT r,FLT z);

/* Main function */
void FCT(FLT *rgeom, FLT *zgeom, FLT *amino, FLT *epsil, FLT *kappa, FLT *delta, FLT *deltl, FLT *deltu,
        FLT *rrmax, FLT *zrmax, FLT *rrmin, FLT *zrmin, FLT *rzmax, FLT *zzmax, FLT *rzmin, FLT *zzmin,
        FLT *rq, FLT *zq, FLT rB, FLT zB, FLT dr, FLT dz,
        size_t npq, size_t noq){
// Returns shape parameters based on array[noq,npq] of contour points rq,zq, boundary point rB, zB
#ifdef DEBUG
 printf("\n\n===Entering shap.c! Hello!===\n");
 printf("rB=%f\n",rB);
#endif
 
 FLT am, rg, dl, du, isLCFS;
 FLT* ptrq = rq;
 FLT* ptzq = zq;
 
 for (int ipq=0; ipq<npq; ipq++,
         ++rrmax,++zrmax,++rrmin,++zrmin,
         ++rzmax,++zzmax,++rzmin,++zzmin){ /* loop over separate contours*/
  if (ipq<npq-1){
   isLCFS=0;
  }
  else {
   isLCFS=1;
  }
  
  for (int ioq=0; ioq<noq; ioq++,++ptrq,++ptzq){ /* loop over 1 contour*/
   
#ifdef DEBUG2
   printf("rq[%d,%d],zq[%d,%d]=[%f,%f]\n",ioq+1,ipq+1,ioq+1,ipq+1,*ptrq,*ptzq);
#endif
   
   if (ioq==0){ /* init */
    *rrmax=*ptrq;*zrmax=*ptzq;
    *rrmin=*ptrq;*zrmin=*ptzq;
    *rzmax=*ptrq,*zzmax=*ptzq;
    *rzmin=*ptrq,*zzmin=*ptzq;
   }
   else{
    if (*ptrq > *rrmax){*rrmax=*ptrq;*zrmax=*ptzq;};
    if (*ptrq < *rrmin){*rrmin=*ptrq;*zrmin=*ptzq;};
    if (*ptzq > *zzmax){*zzmax=*ptzq;*rzmax=*ptrq;}
    if (*ptzq < *zzmin){*zzmin=*ptzq;*rzmin=*ptrq;}
   }
  } /* loop over 1 contour*/
  // refine by either substituting with rB,zB or fitting circle through 3 points
  REFINE(rrmax, zrmax, rq, zq, noq, isLCFS, rB, zB, dr, dz, +1,  0);
  REFINE(rrmin, zrmin, rq, zq, noq, isLCFS, rB, zB, dr, dz, -1,  0);
  REFINE(rzmax, zzmax, rq, zq, noq, isLCFS, rB, zB, dr, dz,  0, +1);
  REFINE(rzmin, zzmin, rq, zq, noq, isLCFS, rB, zB, dr, dz,  0, -1);
  
  rg = (*rrmax+*rrmin)*0.5;
  am = (*rrmax-*rrmin)*0.5;
  du = (rg-*rzmin) / am;
  dl = (rg-*rzmax) / am;
  
  rgeom[ipq] = rg;
  zgeom[ipq] = (*zzmax+*zzmin) * 0.5;
  amino[ipq] = am;
  epsil[ipq] = am/rg;
  kappa[ipq] = (*zzmax-*zzmin) / (*rrmax-*rrmin);
  deltl[ipq] = du;
  deltu[ipq] = dl;
  delta[ipq] = (du + dl) * 0.5;
  
#ifdef DEBUG
  printf("Result for ipq=%d:\n",ipq+1);
  printf("[rrmax,zzmax] = [%f,%f]\n",*rrmax,*zzmax);
  printf("[rrmin,zzmin] = [%f,%f]\n",*rrmin,*zzmin);
  printf("[rzmin,rzmax] = [%f,%f]\n",*rzmax,*zzmax);
  printf("[rzmin,rzmax] = [%f,%f]\n",*rzmin,*zzmin);
  
  printf("[rgeom,zgeom] = [%f,%f]\n",rgeom[ipq],zgeom[ipq]);
  printf("[amino,epsil] = [%f,%f]\n",amino[ipq],epsil[ipq]);
  printf("[kappa,delta] = [%f,%f]\n",kappa[ipq],delta[ipq]);
  printf("[deltl,deltu] = [%f,%f]\n",deltl[ipq],deltu[ipq]);
#endif
  
  rq = ptrq; zq = ptzq; // Update pointers for next contour
 } /* loop over separate contours*/
}

void REFINE(FLT* r1, FLT* z1, FLT* rq, FLT* zq, int noq, int isLCFS, FLT rB, FLT zB, FLT dr, FLT dz, FLT drd, FLT dzd){
// refines extremum point estimates by fitting circle through 3 points
// or substituting with rB, zB where appropriate
// dr dz define direction in which to seek refinement.
 
 FLT r2, z2, r3, z3, rc, zc, dc;
 int s;
 
 // if isLCFS && Diverted &&
 // (rB,zB is extremum in right direction || rB,zB is very close to current extremum)
 if (isLCFS && (rB!=-1) && ( drd*(rB-*r1)>0 || (dzd*(zB-*z1))>0 ||
         (pow(rB-*r1,2)<(dr*dr) && pow(zB-*z1,2)<(dz*dz)) ) ) {
#ifdef DEBUG
  printf("Replaced [r1,z1]=[%f,%f]\n",*r1,*z1);
  printf("By       [rB,zB]=[%f,%f]\n",rB,zB);
#endif
  *r1=rB; *z1=zB; // boundary point replaces this point
  return;
 }
 else {
  FIND_CLOSEST(&r2,&z2,&r3,&z3,rq,zq,noq,*r1,*z1);
  FITCIRC(&rc,&zc,&dc,&s,
          *r1,*z1,r2,z2,r3,z3);
  if (s!=0){
#ifdef DEBUG
   printf("Circle fitting failed\n");
#endif
   return;
  }
  // otherwise take extremum point of circle
  *r1 = rc + drd*(dc);
  *z1 = zc + dzd*(dc);
#ifdef DEBUG
  printf("Refined [r1,z1]=[%f,%f]\n",*r1,*z1);
#endif
 }
}

void FITCIRC(FLT* xc, FLT* yc, FLT* rc, int* s,
        FLT x1, FLT y1, FLT x2, FLT y2, FLT x3, FLT y3){
 // fit a circle (xc,yc), radius rc.  s: Status=0 if failure
 FLT d1x, d2x, d1y, d2y, e1, e2, m1, m2, det, idet;
 // Select which combination of two vectors to show
 if (x2!=x1){
  d1x = x2-x1;
  d1y = y2-y1;
  e1 = (pow(x2,2)-pow(x1,2) + pow(y2,2)-pow(y1,2))*0.5;
  if (x3!=x2){
   d2x = x3-x2; d2y = y3-y2;
   e2 = (pow(x3,2)-pow(x2,2) + pow(y3,2)-pow(y2,2))*0.5;
  }
  else{
   d2x = x3-x1; d2y = y3-y1;
   e2 = (pow(x3,2)-x1*x1 + pow(y3,2)-pow(y1,2))*0.5;
  }
 }
 else{
  if ((x3!=x2) && (x3!=x1)){
   d1x = (x3-x2); d1y = (y3-y2);
   e1 = (pow(x3,2)-pow(x2,2) + pow(y3,2)-pow(y2,2))*0.5;
   
   d2x = (x3-x1); d2y = (y3-y1);
   e2 = (pow(x3,2)-pow(x1,2) + pow(y3,2)-pow(y1,2))*0.5;
  }
  else{
#ifdef DEBUG
   printf("invalid set of points\n");
#endif
   *xc=x1; *yc=y1; *rc=0; *s=1;return;
  }
 }
 
 det = (d1x*d2y-d1y*d2x);
 if(det==0){
#ifdef DEBUG
  printf("Points may not be colinear!\n");
  printf("[%5.3f,%5.3f],[%5.3f,%5.3f],[%5.3f,%5.3f]\n",x1,y1,x2,y2,x3,y3);
#endif
  *xc=x1; *yc=y1; *rc=0; *s=2;return;
 }
 
 idet = 1./det;
 *xc = (+d2y*e1-d1y*e2) * idet;
 *yc = (-d2x*e1+d1x*e2) * idet;
 *rc = sqrt( pow(x1-*xc,2) + pow(y1-*yc,2) );
 *s=0;
#ifdef DEBUG
 printf("fitted to [%4.2f,%4.2f],[%4.2f,%4.2f],[%4.2f,%4.2f]\n",x1,y1,x2,y2,x3,y3);
 printf("found [xc,yc]=[%4.2f,%4.2f], rc=%4.2f\n",*xc,*yc,*rc);
#endif
 
}

void  FIND_CLOSEST(FLT* r1,FLT* z1,FLT* r2,FLT* z2,FLT* rq, FLT* zq,int noq,FLT r,FLT z){
// Find two closest points (r1,z1), (r2,z2) to r,z out of list of points rq,zq
 
#ifdef DEBUG
 printf("\n\n** Finding points close to [r1,z1]=[%f,%f], nop=%d **\n",r,z,noq);
#endif
 
 FLT sqdist, sqdistmin1, sqdistmin2;
 int nass = 0; // track how many were already assigned
 
 for(int ii=0;ii<noq;ii++){
  sqdist = pow(r-(rq[ii]),2) + pow(z-(zq[ii]),2);
#ifdef DEBUG2
  printf("\nii=%d, [rq,zq]=[%4.2f,%4.2f], dist=%4.2e\n",ii,rq[ii],zq[ii],sqdist);
#endif
  if (sqdist==0){
#ifdef DEBUG2
   printf("target point itself, skip\n");
#endif
   continue;}; // skip if the point is r1,z1 itself
   
   // Unconditinally assign first two nonzero distances
   if(nass==0){
    sqdistmin1 = sqdist; *r1=rq[ii]; *z1=zq[ii]; nass++;
#ifdef DEBUG2
    printf("assign first\n");
#endif
    continue;
   }
   
#ifdef DEBUG2
   printf("d=%4.2e, d1=%4.2e, d2=%4.2e",sqdist,sqdistmin1,sqdistmin2);
#endif
   // If sqdist<sqdistmin2 or sqdistmin2 not yet assigned
   if (sqdist<sqdistmin2 || nass==1){
    nass++;
    if (sqdist<sqdistmin1){
     // update point2=point1, point1=newpoint
     *r2=*r1; *z2=*z1;
     *r1=rq[ii]; *z1=zq[ii];
     
     sqdistmin2 = sqdistmin1;
     sqdistmin1 = sqdist;
#ifdef DEBUG2
     printf("...update p1,p2=[%6.2f,%6.2f],[%6.2f,%6.2f],[[d1,d2]=[%4.2e,%4.2e]\n",*r1,*z1,*r2,*z2,sqdistmin1,sqdistmin2);
#endif
    }
    else{
     // update point2=newpoint, keep point1
     *r2=rq[ii]; *z2=zq[ii];
     sqdistmin2 = sqdist;
#ifdef DEBUG2
     printf("...update p2=[%6.2f,%6.2f], d2=%4.2e\n",*r2,*z2,sqdistmin2);
#endif
    }
   }
   else{
#ifdef DEBUG2
    printf("...no update\n");
#endif
   }
 }
#ifdef DEBUG
 printf("Found close points [r2,z2]=[%f,%f], [r3,z3]=[%f,%f]\n",*r1,*z1,*r2,*z2);
#endif
}
