pro nlfff_fit_v1,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
;
; Contact     : aschwanden@lmsal.com
;-

t1      =systime(0,/seconds)

print,'___________________________NLFFF_FIT_____________________________'
restore,dir+savefile    	;--> PARA, INPUT
instr	=input.instr
fov0    =input.fov0      ;field-of-view
nsm1    =input.nsm1      ;lowpass filter
nmag_p  =input.nmag_p    ;number of magnetic charges
nmag_np =input.nmag_np   ;number of magnetic charges
amis	=input.amis
nitmin	=input.nitmin

filename=strmid(savefile,0,15)
dateobs	=para.dateobs
x1_sun  =para.x1_sun
x2_sun  =para.x2_sun
y1_sun  =para.y1_sun
y2_sun  =para.y2_sun
wave_	=para.wave_

da0	=1.0  
wave 	=wave_(0)
nitmax	=100 
ds      =0.002          ;spatial resolution along loop
nsmax   =long(fov0/ds)  ;max number of loop points
fov	=[x1_sun,y1_sun,x2_sun,y2_sun]
nh	=6
nseg	=9
hmin	=0.0
hmax	=0.05
hrange	=(hmax -hmin)

;_________________EXTRACT LOOP COORDINATES AND VECTORS______________ 
nf	  =n_elements(ns_loop_det)		    ;number of loops
if (nf eq 0) then goto,save_data
ns_loop   =ns_loop_det
wave_loop =wave_loop_det
field_loop=field_loop_det
nfit    =nf*nseg
len     =fltarr(nf)                                 ;proj loop length
s_fit   =fltarr(nfit)                               ;fit spline points
x_fit   =fltarr(nfit,3)                             ;fit spline points
v_fit   =fltarr(nfit,3)                             ;fit spline points
s_len   =fltarr(nsmax,nf)                           ;proj length coord s(x,y)
xl	=fltarr(nsmax)
yl	=fltarr(nsmax)
zl	=fltarr(nsmax)

for k=0,nf-1 do begin                               ;number of loops
 ns	 =ns_loop[k] < nsmax
 xl[0:ns-1]=field_loop[0:ns-1,0,k]
 yl[0:ns-1]=field_loop[0:ns-1,1,k]
 zl[0:ns-1]=field_loop[0:ns-1,2,k]
 for is=1,ns-1 do s_len(is,k)=s_len(is-1,k)+$
  sqrt((xl(is)-xl(is-1))^2+(yl(is)-yl(is-1))^2)
 len[k] =s_len[ns-1,k]
 for iseg=0,nseg-1 do begin
  if (nseg ge 2) then im=long(1+(ns-3)*float(iseg)/float(nseg-1)) < (ns-2)
  if (nseg eq 1) then im=1
  ifit  =k*nseg+iseg
  s_fit[ifit]   =s_len(im,k)
  x_fit[ifit,0] =xl[im]
  x_fit[ifit,1] =yl[im]
  x_fit[ifit,2] =zl[im]
  v_fit[ifit,0] =xl[im+1]-xl[im-1]                  ;vector x-component
  v_fit[ifit,1] =yl[im+1]-yl[im-1]                  ;vector y-component
  v_fit[ifit,2] =zl[im+1]-zl[im-1]                  ;vector y-component
 endfor
endfor

;________________LOOP GEOMETRIES_____________________________
nc	=long(nh)
nn	=long(nc*nh*(nh-1))	
h_curv  =fltarr(nn)
s_curv  =fltarr(nn)
r_curv  =fltarr(nn)
ns	=200					;typical length
s_seg	=(long(1+(ns-3)*findgen(iseg)/float(nseg-1)) < (ns-2))/float(ns)
h_seg_norm=fltarr(nseg,nn)
disp	=0
eps	=1.e-3
ii	=0
for ia=0,1 do begin				;conjugate footpoints
 for ih2=0,nh-1 do begin
 for ih1=0,ih2-1 do begin
  qh1	=float(ih1)/float(nh-1)
  qh2	=float(ih2)/float(nh-1) 		;altitude levels
  for ic=0,nc-1 do begin			;curvature radii
   th	=atan(qh2)                              ;inclination angle
   d	=sqrt((1./2.)^2+(qh2/2.)^2)
   r_curv1=-1/sin(th > eps)			;neg curv radius limit
   r_curv2=+d/cos(th)				;pos curv radius limit
   q_curv1=1./r_curv1				;reciprocal value
   q_curv2=1./r_curv2				;reciprocal value
   q_curv =q_curv1+(q_curv2-q_curv1)*float(ic)/float(nc-1)
   r_curv[ii]=1./q_curv
   dd   =sqrt(r_curv[ii]^2-d^2)
   sign_curv=r_curv[ii]/abs(r_curv[ii])
   s_curv[ii]=1./2. +dd*sin(th)*sign_curv
   h_curv[ii]=qh2/2.-dd*cos(th)*sign_curv
   root =r_curv[ii]^2-(s_curv[ii]-s_seg)^2  
   h_seg0=(h_curv[ii]+sqrt(root)*sign_curv) > 0.
   h_seg1=qh1+(qh2-qh1)*h_seg0
   if (ia eq 0) then h_seg_norm(*,ii)=h_seg1
   if (ia eq 1) then h_seg_norm(*,ii)=reverse(h_seg1)
;...........................TEST DISPLAY......................
   if (test eq 2) then begin
    if (disp eq 0) then begin
     window,0,xsize=512,ysize=512
     clearplot
     loadct,5
     plot,[0,1],[0,0],yrange=[0,1],xrange=[0,1]
     disp=1
    endif
    oplot,s_seg,h_seg_norm(*,ii),color=55.+200*(float(ic)+0.5)/float(nc)
   endif
;.............................................................
   ii	=ii+1
  endfor
  endfor
 endfor
endfor
print,'Number of geometries =      ',nn
if (test eq 2) then begin
 read,'Continue? [0,1] : ',yes
 if (yes eq 0) then STOP
endif

;______________________ITERATION START________________________ 
angle   =90.
slope   =-1.
coeff_best=coeff
angle_iter=fltarr(nitmax>1)
if (nitmax eq 0) then goto,end_iter
for iter=0,nitmax-1 do begin

;_______________________ALTITUDE EVALUATION______________________
 if (iter eq 0) then begin
  h_apex =fltarr(nf)
  xx_fit=fltarr(nfit,3,nn)
  vv_fit=fltarr(nfit,3,nn)
  for ii=0,nn-1 do begin                           ;geometries
   for k=0,nf-1 do begin                           ;loops
    ifit =k*nseg+findgen(nseg)
    ifit =k*nseg+findgen(nseg)
    i1fit=k*nseg+((findgen(nseg)-1)>0)
    i2fit=k*nseg+((findgen(nseg)+1)<(nseg-1))
    i3fit=k*nseg
    i4fit=k*nseg+(nseg-1)
    h_seg=h_seg_norm(*,ii)*(len(k) < hrange)       ;MODIFIED 2015-Oct-8
    r_seg=1.0+hmin+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)
    vv_fit[ifit,0,ii]=xx_fit[i2fit,0,ii]-xx_fit[i1fit,0,ii]
    vv_fit[ifit,1,ii]=xx_fit[i2fit,1,ii]-xx_fit[i1fit,1,ii]
    vv_fit[ifit,2,ii]=xx_fit[i2fit,2,ii]-xx_fit[i1fit,2,ii]
    vv_fit[i3fit,2,ii]=vv_fit[i3fit+1,2,ii]
    vv_fit[i4fit,2,ii]=vv_fit[i4fit-1,2,ii]
    h_apex(k)=max(h_seg)
   endfor
  endfor
 endif

 dev_geo=fltarr(nf)+90.
 dev_geo_nseg=fltarr(nseg,nf)+90.
 for ii=0,nn-1 do begin                             ;looping geometries
  nlfff_vector,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_deg=(180./!pi)*dev_rad                        ;radian into degrees
  for k=0,nf-1 do begin
   ifit =k*nseg+findgen(nseg)
   dev_med=median(dev_deg(ifit))                    ;mean square misalign angle
   if (dev_med lt dev_geo(k)) then begin
    dev_geo(k)=dev_med
    dev_geo_nseg(*,k)=dev_deg(ifit)
    x_fit(ifit,0)=xx_fit[ifit,0,ii]                 ;update x-coordinate
    x_fit(ifit,1)=xx_fit[ifit,1,ii]                 ;update y-coordinate
    x_fit(ifit,2)=xx_fit[ifit,2,ii]                 ;update z-coordinate
    v_fit(ifit,0)=vv_fit[ifit,0,ii]                 ;update x-vector
    v_fit(ifit,1)=vv_fit[ifit,1,ii]                 ;update y-vector
    v_fit(ifit,2)=vv_fit[ifit,2,ii]                 ;update z-vector
    ns  =ns_loop[k]
    field_loop[0:ns-1,2,k]=interpol(x_fit[ifit,2],s_fit[ifit],s_len[0:ns-1,k])
   endif
  endfor
 endfor

 if (test eq 2) then begin
  for k=0,nf-1 do begin
   print,dev_geo_nseg(*,k)
   print,'median misalignment = ',dev_geo(k)
   nlfff_fit_disp,x_det,s_det,k,para,coeff
  endfor
 endif

;______________MAXIMUM MISALIGNMENT LIMIT_____________________________
 if (iter eq 0) then begin
  ind_good0=where(dev_geo le amis,ngood0)  
  nf0	   =nf					;initial loop number
 endif
 qiter	    =(1.-float(iter)/float(nitmin))>0.
 ngood      =long(ngood0+(nf0-ngood0)*qiter)
 isort	    =sort(dev_geo)
 amis_ngood =dev_geo(isort(ngood-1))
 ind_good   =where(dev_geo le amis_ngood,ngood)	;loop indices
 nf	    =ngood
 
;______________SELF-SELECTION OF GOOD LOOPS____________________________
 len_old =len
 s_len_old=s_len
 s_old   =s_fit
 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)
 len     =len_old(ind_good)
 s_len   =s_len_old(*,ind_good)
 nfit    =nf*nseg
 s_fit   =fltarr(nfit)                             ;fit spline points
 x_fit   =fltarr(nfit,3)                           ;fit spline points
 v_fit   =fltarr(nfit,3)                           ;fit spline points
 xx_fit  =fltarr(nfit,3,nn)                        ;fit spline points
 vv_fit  =fltarr(nfit,3,nn)                        ;fit spline points
 for k=0,nf-1 do begin
  kold =ind_good(k)
  iold =nseg*kold+findgen(nseg)
  ifit =nseg*k   +findgen(nseg)
  s_fit(ifit)=s_old(iold)
  x_fit(ifit,*)=x_old(iold,*)
  v_fit(ifit,*)=v_old(iold,*)
  xx_fit(ifit,*,*)=xx_old(iold,*,*)
  vv_fit(ifit,*,*)=vv_old(iold,*,*)
 endfor

;______________________ALPHA GRADIENT OPTIMIZATION_____________
 grad_alpha=fltarr(nmag_np)
 for im=0,nmag_np-1 do begin
  coeff2=coeff
  coeff2(im,4)=coeff(im,4)+da0
  nlfff_vector,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
 grad_norm=max(abs(grad_alpha))	
 if (grad_norm ne 0) then grad_alpha=grad_alpha/grad_norm
 if (grad_norm eq 0) or (nmag_np eq 1) then grad_alpha(0)=-1.
 coeff(0:nmag_np-1,4)=coeff(0:nmag_np-1,4)+da0*grad_alpha(0:nmag_np-1)

;______________________NEW_MISALIGNMENT ANGLE_________________
 nlfff_vector,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_deg1=(180./!pi)*dev_rad                    ;radian into degrees
 dev_med1=median(dev_deg1)
 angle_iter[iter]=dev_med1

 if (dev_med1 lt angle) then begin 
  angle	=dev_med1
  dev_deg_best=dev_deg1				;array
  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,median(dev_deg_best),slope,format='(A,2I6,f8.2,f9.3)'
 if (slope ge -0.1) and (iter ge nitmin) then goto,end_iter
endfor  		;for iter=0,nitmax-1
END_ITER:
coeff	=coeff_best
dev_deg =dev_deg_best

;_______________________2D MISALIGNMENT ANGLE__________________
vector_product_array2,v_fit,bfff_,v3,v3_norm,angle_rad2  
dev_rad2=angle_rad2 < (!pi-angle_rad2)           ;180-deg ambiguity
dev_deg2=(180./!pi)*dev_rad2                    ;radian into degrees
dev_med2=median(dev_deg2)   			;mean misalign angle 
angle2	=dev_med2

;_______________________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.angle  =angle
para.angle2 =angle2
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
