pro nlfff_fit_qelim,dir,savefile,test
;+
; Project     : SOHO/MDI, SDO/HMI, STEREO 
;
; Name        : NLFFF_FIT
;
; Category    : Magnetic data modeling 
;
; Explanation : Fitting of twisted field lines (nonlinear force-free
;		field) NLFFF-model to observed loop projections.
;
; Syntax      : IDL>nlfff_fit,dir,savefile,test
;
; Inputs      : dir,savefile,test
;
; Outputs     ; updates savefile
;
; History     : 16-May-2011, Version 1 written by Markus J. Aschwanden
;             : 17-Dec-2015, Successful test free energy step flare 2014-Mar-29
;	      ;  2-Jul-2019, new analytical solution NLFFF_VECTOR.PRO
;	      ;  7-Oct-2019, independent fit in each time interval, no smoothing
;	      ;  7-Oct-2019, initial condition coeff2(im,4)=0 	
;	      ; 15-Oct-2019, eliminate weight 
;	      ;  8-Nov-2019, add index.nh,nseg,hmin,hmax,nitmin,nitmax
;	      ;  8-Nov-2019, eliminate grad_norm,s_len,s_fit,xl,yl,zl
;	      ;  9-Nov-2019, curved loop geometries h(s) 
;	      ; 11-Nov-2019, calculate vector directions with NLFFF_FIT_DIR
;	      ; 14-Nov-2019, replace amis (misaligned) with qelim (eliminateed)
;	      ; 22-Dec-2020, nmag_np_var=(long(nmag_np*iter/nitmin)>1)<nmag_np  
;
; Contact     : aschwanden@lmsal.com
;-

t1      =systime(0,/seconds)

print,'___________________________NLFFF_FIT_____________________________'
restore,dir+savefile            ;--> PARA, INPUT
instr   =input.instr
nmag_p  =input.nmag_p    ;number of magnetic charges
nmag_np =input.nmag_np   ;number of magnetic charges
amis	=input.amis
qelim   =input.qelim
nitmin  =input.nitmin
nitmax  =input.nitmax
nh      =input.nh
nseg    =input.nseg
ncurv   =input.ncurv
hmin    =input.hmin
hmax    =input.hmax
hrange  =(hmax-hmin)
nn      =ncurv*nh*2
eps	=0.001

;________________LOOP GEOMETRIES_____________________________
s_norm	=findgen(nseg)/float(nseg-1)
h_seg_norm=fltarr(nseg,nn)
ii      =0
for ia=0,1 do begin                             ;conjugate footpoints
 for ih=0,nh-1 do begin
  for icurv=0,ncurv-1 do begin 
   qh   =float(ih)/float(nh-1)                ;altitude levels
   s1	=0.
   s2   =(float(icurv)+1)/float(ncurv)
   s_	=s1+(s2-s1)*findgen(nseg)/float(nseg-1)
   h_norm=qh-(4*qh)*(s_-0.5)^2
   if (ia eq 0) then h_seg_norm(*,ii)=h_norm
   if (ia eq 1) then h_seg_norm(*,ii)=reverse(h_norm)
;.................TEST DISPLAY................................
;  if (ii eq 0) then begin
;   window,1,xsize=512,ysize=512
;   clearplot
;   loadct,5
;   plot,[0,1],[0,0],yrange=[0,1],xrange=[0,1]
;  endif
;  col=long(20.+200*findgen(nn)/float(nn-1))
;  oplot,s_norm,h_seg_norm(*,ii),color=col(ii)
;.............................................................
   ii	=ii+1
  endfor
 endfor
endfor
print,'Number of loop geometries =      ',nn

;_________________INITIAL LOOP COORDINATES AND VECTORS______________ 
nf	 =n_elements(ns_loop_det)		    ;number of loops
nfit     =nf*nseg
ns_loop   =ns_loop_det
wave_loop =wave_loop_det
field_loop=field_loop_det
if (nf eq 0) then goto,save_data

x_fit   =fltarr(nfit,3)                             ;fit spline points
v_fit   =fltarr(nfit,3)                             ;fit vectors
xx_fit  =fltarr(nfit,3,nn)                          ;height profile models h(s)
vv_fit  =fltarr(nfit,3,nn)                          ;vector profile models v(s)
len	=fltarr(nf)

for k=0,nf-1 do begin                               ;number of loops
 nsmax =max(ns_loop)
 ns    =ns_loop(k) 
 ifit  =k*nseg+findgen(nseg)
 is    =long(1+(ns-3)*findgen(nseg)/float(nseg-1)) 
 x_fit(ifit,0)=field_loop(is,0,k)
 x_fit(ifit,1)=field_loop(is,1,k)
 x_fit(ifit,2)=sqrt((1.+hmin+eps)^2-x_fit(ifit,0)^2-x_fit(ifit,1)^2)
 x	=reform(x_fit(ifit,*))
 nlfff_fit_dir,x,v
 v_fit(ifit,*)=v 

 dx	=x_fit(ifit(nseg-1),0)-x_fit(ifit(0),0)
 dy	=x_fit(ifit(nseg-1),1)-x_fit(ifit(0),1)
 len(k) =sqrt(dx^2+dy^2)

 for ii=0,nn-1 do begin                            ;geometries
  h_seg=h_seg_norm(*,ii)*(len(k) < hrange)	     
  r_seg=1.0+hmin+eps+h_seg
  xx_fit(ifit,0,ii)=x_fit(ifit,0)
  xx_fit(ifit,1,ii)=x_fit(ifit,1)
  xx_fit(ifit,2,ii)=sqrt(r_seg^2-x_fit(ifit,0)^2-x_fit(ifit,1)^2)
  xx	=reform(xx_fit(ifit,*,ii))
  nlfff_fit_dir,xx,vv 
  vv_fit(ifit,*,ii)=vv
 endfor
endfor

;______________________ITERATION START________________________ 
angle   =90.
angle2	=90.
slope   =-1.
coeff(*,4)=0.					;reset a0
coeff_best=coeff
angle_iter=fltarr(nitmax>1)
dev_geo=fltarr(nf)+90.
if (nitmax eq 0) then goto,end_iter
for iter=0,nitmax-1 do begin

;______________________ALTITUDE EVALUTATION____________________
 for ii=0,nn-1 do begin                             ;looping geometries
  nlfff_vector_v3,coeff,xx_fit(*,*,ii),bfff_        ;B-field at pos x_fit
  vector_product_array, vv_fit(*,*,ii),bfff_,v3,v3_norm,a_rad
  dev_rad=a_rad < (!pi-a_rad)                       ;180-deg ambiguity
  dev_arr=(180./!pi)*dev_rad                        ;radian into degrees
  for k=0,nf-1 do begin
   ifit =k*nseg+findgen(nseg)
   dev_med=median(dev_arr(ifit(1:nseg-2)))         ;median (w/o first and last) 
   if (dev_med lt dev_geo(k)) then begin
    dev_geo(k)=dev_med
    ns  =ns_loop(k)
    x_fit(ifit,*)=xx_fit(ifit,*,ii)		    ;update x_fit and v_fit
    nlfff_fit_dir,x_fit(ifit,*),v 
    nlfff_fit_dir,xx_fit(ifit,*,ii),vv 
    v_fit(ifit,*)=v
    vv_fit(ifit,*)=vv
    iseg=float(findgen(nseg))
    ipix=float((nseg-1)*findgen(ns)/float(ns-1))
    field_loop(0:ns-1,2,k)=interpol(x_fit(ifit,2),iseg,ipix)
   endif
  endfor
 endfor

;______________MISALIGNMENT SELECTION____________________________
 if (iter eq 0) then nf0=nf 
 nf1	    =long(nf0*(1.-qelim))
 ngood 	    =long(nf0+(nf1-nf0)*float(iter)/float(nitmin-1)) > nf1
 isort	    =sort(dev_geo)
 ind_good   =isort(0:ngood-1)
 nf	    =ngood

;______________SELF-SELECTION OF GOOD LOOPS____________________________
 x_old	=x_fit
 v_old	=v_fit
 xx_old	=xx_fit
 vv_old	=vv_fit
 ns_loop   =ns_loop(ind_good)
 wave_loop =wave_loop(ind_good)
 field_loop=field_loop(*,*,ind_good)
 dev_geo =dev_geo(ind_good)
 len	=len(ind_good)
 nfit   =nf*nseg				;new value of nf and nfit	
 x_fit	=fltarr(nfit,3)
 v_fit	=fltarr(nfit,3)
 xx_fit	=fltarr(nfit,3,nn)
 vv_fit	=fltarr(nfit,3,nn)
 for k=0,nf-1 do begin
  kold =ind_good(k)
  iold =nseg*kold+findgen(nseg)
  ifit =nseg*k   +findgen(nseg)
  x_fit(ifit,*)=x_old(iold,*)
  v_fit(ifit,*)=v_old(iold,*)
  for ii=0,nn-1 do begin
   xx_fit(ifit,*,ii)=xx_old(iold,*,ii)
   vv_fit(ifit,*,ii)=vv_old(iold,*,ii)
  endfor
 endfor

;______________________ALPHA GRADIENT OPTIMIZATION_____________
 nmag_np_var=(long(nmag_np*iter/nitmin)>1)<nmag_np ;linear increase  
 grad_alpha=fltarr(nmag_np_var)
 for im=0,nmag_np_var-1 do begin
  coeff2=coeff
  nlfff_vector_v3,coeff2,x_fit,bfff_              ;B-field at position x_fit
  vector_product_array  ,v_fit,bfff_,v3,v3_norm,angle_rad   
  dev_rad =angle_rad < (!pi-angle_rad)           ;180-deg ambiguity
  dev_deg0=(180./!pi)*dev_rad                    ;radian into degrees
  dev_med0=median(dev_deg0)
  grad_alpha[im]=angle-dev_med0
 endfor
 coeff(0:nmag_np_var-1,4)=coeff(0:nmag_np_var-1,4)+grad_alpha(0:nmag_np_var-1)

;______________________NEW_MISALIGNMENT ANGLE_________________
 nlfff_vector_v3,coeff,x_fit,bfff_                 ;B-field at position x_fit
 vector_product_array ,v_fit,bfff_,v3,v3_norm,angle_rad  
 dev_rad=angle_rad < (!pi-angle_rad)               ;180-deg ambiguity
 dev_arr=(180./!pi)*dev_rad                        ;radian into degrees
 dev_deg_k=fltarr(nf)
 for k=0,nf-1 do begin
  ifit=k*nseg+findgen(nseg)
  dev_deg_k(k)=median(dev_arr(ifit(1:nseg-2)))      ;median (w/o first and last) 
 endfor 
 dev_med=median(dev_deg_k)
 angle_iter(iter)=dev_med
 if (dev_med lt angle) then begin 
  angle	=dev_med
  dev_deg=dev_deg_k	
  coeff_best=coeff
 endif

;_______________________CONVERGENCE CRITERION__________________
 if (iter ge 1) then begin
  iter1	=(iter-nitmin-1)>0
  iter_ =findgen(nitmax)
  c	=linfit(iter_(iter1:iter),angle_iter(iter1:iter))
  slope	=c(1)
 endif
 print,'ITER, NLOOP, MISALIGN, SLOPE --> ',$
  iter,ngood,angle,slope,format='(A,2I6,2f8.2)'
 if (slope ge -0.1) and (iter ge nitmin) then goto,end_iter
endfor  		;for iter=0,nitmax-1
END_ITER:
coeff	=coeff_best

;_______________________2D MISALIGNMENT ANGLE__________________
nlfff_vector_v3,coeff,x_fit,bfff_                 ;B-field at position x_fit
vector_product_array2,v_fit,bfff_,v3,v3_norm,angle_rad  
dev_rad =angle_rad < (!pi-angle_rad)              ;180-deg ambiguity
dev_arr =(180./!pi)*dev_rad                       ;radian into degrees
dev_deg2_k=fltarr(nf)
for k=0,nf-1 do begin
 ifit=k*nseg+findgen(nseg)
 dev_deg2_k(k)=median(dev_arr(ifit(1:nseg-2)))     ;median (w/o first and last segm) 
endfor 
dev_med2=median(dev_deg2_k)
if (dev_med2 lt angle2) then begin 
 angle2=dev_med2
 dev_deg2=dev_deg2_k
endif

;_______________________CPU time_________________________________
t2     =systime(0,/seconds)
cpu    =t2-t1
print,'FORWARD FITTING : CPU time        = ',cpu,format='(a,f8.1)'

;_______________________SAVE PARAMETERS________________________
SAVE_DATA:
para.iter   =iter
para.cpu    =cpu
para.nitmax =nitmax
save,filename=dir+savefile,input,para,bzmap,bzfull,bzmodel,coeff,$
     field_loop_det,ns_loop_det,wave_loop_det,$
     field_loop,ns_loop,wave_loop,dev_deg,dev_deg2,angle,angle2
print,'parameters saved in file = ',savefile
end
