;
; XFORMTHEO - transform theodolite data to consistent world system
;
; REF_STR - string data containing positions of reference points
;           FORMAT (each line):  "NAME XMEAS YMEAS ZMEAS XNOM YNOM ZNOM"
;
;           NAME - 17 character string - must be blank padded! (ignored)
;           X/Y/ZMEAS - measured positions of reference points (inches)
;           X/Y/ZNOM  - desired positions of reference points
;                       (inches)
;
;           These should be the coordinates of known targets.  In the
;           case of the BAT, the nominal positions should be the
;           "mask" coordinates used by the mechanical people, namely
;           X/Y/Z as measured from the CZT center-plane.  (NOT the
;           same as BAT_X/Y/Z).
;
; DAT_STR - string data containing positions of measured points
;           FORMAT (each line):  "NAME XMEAS YMEAS ZMEAS"
;           NAME - 17 character string - must be blank padded!
;           X/Y/ZMEAS - measured positions of named targets
;
; P0 - initial transform parameters (see XFORM3D)
;
; PTRANSFORM - final fitted transform parameters (see XFORM3D)
;
; DPTRANSFORM - standard errors of PTRANSFORM, assuming delta(chi^2)=1
;
; POS_ERR - input, the uncertainty in the position measurement along
;           the *measured* axes not the BAT_X/Y/Z axes.
;
; REFPOINTS - output, a structure containing the reference points
;             Fields: NAME, MEAS, NOM
;             Should be unaltered from raw reference point data
;
; DATPOINTS - output, a structure containing the data points
;             Fields: NAME, MEAS, NOM, ENOM, BAT, EBAT
;             NAME and MEAS are same as original data
;             NOM and ENOM are position in mask coordinates, and error
;             BAT and EBAT are position in BAT_X/Y/Z coordinates, and error
; 
;

pro xformtheo, ref_str, dat_str, p0=p0, ptransform=ptransform, $
               dptransform=dptransform, pos_err=pos_err, $
               refpoints=points, datpoints=calpoints0

   cg1_str = ref_str

   ;; Extract data from reference strings
   ;; NAME: point name
   ;; MEAS: measured position
   ;; NOM:  nominal position, as determined a priori
   point_template = {name:'', meas:[0d,0,0], nom:[0d,0,0]}
   points = replicate(point_template, n_elements(cg1_str))
   reads, cg1_str, points, format='((A17,3(D0),3(D0)))'

   ;; Initial parameter values for coordinate transformation
   if n_elements(p0) EQ 0 then $
     p0 = [80d, -190d, 520d, -1d, -135d, 180d]
   if n_elements(pos_err) EQ 0 then begin
       xerr = 0.0022 & yerr = 0.0150 & zerr = 0.0010     
   endif else begin
       xerr = pos_err(0)
       yerr = pos_err(1)
       zerr = pos_err(2)
   endelse

   ;; Construct arrays for fit.  Concatenate X/Y/Z data together
   xyz = [points.meas(0), points.meas(1), points.meas(2)]   ;; Position
   xyz_nom = [points.nom(0), points.nom(1), points.nom(2)]  ;; Nom. position
   xyz_err = [points.nom(0)*0+xerr, points.nom(1)*0+yerr, points.nom(2)*0+zerr]

   pnew = mpfitfun('xform3d', xyz_nom, xyz, xyz_err, p0, $
                   perror=dpnew, yfit=xyz_fit)
   ptransform = pnew
   dptransform = dpnew

   
   ;; Now take real data
   cg1_calstr = dat_str
   calpoint_template = {name:'', meas:[0d,0,0]}
   calpoints = replicate(calpoint_template, n_elements(cg1_calstr))
   reads, cg1_calstr, calpoints, format='((A16,3(D0)))'

   ;; Transform from theodolite coord syst to mask coord syst
   xyz_cal = [calpoints.meas(0), calpoints.meas(1), calpoints.meas(2)]
   xyz_cal_czt = xform3d(xyz_cal, pnew, /invert)
   
   ncal = n_elements(calpoints)
   calpoints0 = replicate({name:'', meas:[0d,0,0], $
                           nom:[0d,0,0], enom:[0d,0,0],$
                           bat:[0d,0,0], ebat:[0d,0,0]}, ncal)

   calpoints0.name = calpoints.name
   calpoints0.meas = calpoints.meas
   calpoints0.nom(0) = xyz_cal_czt(0:ncal-1)
   calpoints0.nom(1) = xyz_cal_czt(ncal:2*ncal-1)
   calpoints0.nom(2) = xyz_cal_czt(2*ncal:*)


   ;; Compute error bars by perturbing the input position and
   ;; transforming
   xyz_ecal = [calpoints.meas(0)+xerr, calpoints.meas(1), calpoints.meas(2)]
   xyz_ecal_czt = (xform3d(xyz_ecal, pnew, /invert) - xyz_cal_czt)^2
   xyz_ecal = [calpoints.meas(0), calpoints.meas(1)+yerr, calpoints.meas(2)]
   xyz_ecal_czt = xyz_ecal_czt + (xform3d(xyz_ecal, pnew, /invert) - xyz_cal_czt)^2
   xyz_ecal = [calpoints.meas(0), calpoints.meas(1), calpoints.meas(2)+zerr]
   xyz_ecal_czt = xyz_ecal_czt + (xform3d(xyz_ecal, pnew, /invert) - xyz_cal_czt)^2
   
   xyz_ecal_czt = sqrt(xyz_ecal_czt)

   calpoints0.enom(0) = xyz_ecal_czt(0:ncal-1)
   calpoints0.enom(1) = xyz_ecal_czt(ncal:2*ncal-1)
   calpoints0.enom(2) = xyz_ecal_czt(2*ncal:*)
   
   calpoints0.bat(0)  = calpoints0.nom(1)  ;; BAT_X = +Y
   calpoints0.bat(1)  = calpoints0.nom(2)  ;; BAT_Y = +Z
   calpoints0.bat(2)  = calpoints0.nom(0)  ;; BAT_Z = +X
   calpoints0.ebat(0) = calpoints0.enom(1) ;; BAT_X = +Y
   calpoints0.ebat(1) = calpoints0.enom(2) ;; BAT_Y = +Z
   calpoints0.ebat(2) = calpoints0.enom(0) ;; BAT_Z = +X
   
   ;; Convert to real BAT coordinates, centimeters, origin, thickness of Ba-133 pkg
   calpoints0.bat = calpoints0.bat*2.54d
   calpoints0.ebat = calpoints0.ebat*2.54d
   
   ;; Theodolite mask coordinate system is measured w.r.t the 
   ;; centerplane of CZT, which is at BAT_Z = (0.15 + 0.10 cm)
   calpoints0.bat(2) = calpoints0.bat(2) + (0.15d + 0.10d)

   return
end