;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
function get_det, all, det_string
;  Return all records where(id_string mat /det_string/)
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
    det = [0]
    for j = 0, n_elements(all)-1 do begin
        if (strpos(all[j].id_string,det_string) ne -1) then begin
            det = [det, j]
        end
    end

    return, all[det[1:*]]
end

;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
function get_val, i_det, angles
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
dims = size(angles, /dimensions)
ang_out = fltarr(dims[1]) - 99999.0

for id = 0, dims[1]-1 do begin
    n_rec = 0
    ang = [0.0]
    for i_rec = 0, dims[2]-1 do begin
        val = angles[i_det, id, i_rec]
        if (val gt -9999.) then ang = [ang, val]
    end
    n_ang = n_elements(ang)-1
    if (n_ang eq 1) then begin 
        ang_out[id] = ang[1] 
    end else begin
        if (n_ang lt 5) then $
          ang_out[id] = mean(ang[1:*]) $
        else $
          ang_out[id] = median(ang[1:*])
    end
end


return, ang_out

end
      
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
function get_ang_array, i_det, id, angles
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
dims = size(angles, /dimensions)

n_rec = 0
ang = [0.0]
for i_rec = 0, dims[2]-1 do begin
    val = angles[i_det, id, i_rec]
    if (val gt -9999.) then ang = [ang, val]
end

return, ang

end
      
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
pro set_initial_values, yag, zag, version
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;

; Positions c.a. July 2001, from ../fid_drift/plot_drift.pro

if (version eq '2001-Jul') then begin
    yag[0, *, 0] = [    940.26481,  -755.44674,   57.905991,   2157.9485,  -1808.4649,   406.08765]
    yag[1, *, 0] = [    938.10850,  -756.50564,   55.148491,   2157.6203,  -1810.4838,   403.39283]
    yag[2, *, 0] = [  -1157.2550 ,  1242.4331 , -1159.9849 ,  1241.1465 , -2.1081770 , -2.1081770 ]
    yag[3, *, 0] = [  -758.76723 ,  853.70469 , -1187.3030 ,  1284.0278 , -2.1081770 , -2.1081770 ]
                                                                                                        
    zag[0, *, 0] = [   -1723.3984,  -1726.9904,  -1855.8921,   181.60785,   175.10916,   818.20534]
    zag[1, *, 0] = [   -829.13964,  -835.10942,  -962.65801,   1070.5223,   1068.7603,   1711.9177]
    zag[2, *, 0] = [  -453.13658 , -445.82445 ,  576.11763 ,  578.57570 , -2.5663324 , -2.5663324 ]
    zag[3, *, 0] = [  -1291.6851 , -1293.6106 ,  1012.8395 ,  1010.4306 , -2.5663324 , -2.5663324 ]
end

if (version eq '2001-Jul-iter') then begin
    yag[0,*,0] = [    940.34,   -755.44,     57.77,   2157.92,  -1808.47,    406.35]
    yag[1,*,0] = [    937.58,   -756.64,     54.95,   2157.72,  -1810.42,    403.57]
    yag[2,*,0] = [  -1157.26,   1242.45,  -1159.92,   1241.07,      0.00,      0.00]
    yag[3,*,0] = [   -758.82,    853.82,  -1187.27,   1284.02,      0.00,      0.00]
    zag[0,*,0] = [  -1722.91,  -1727.05,  -1856.11,    181.57,    175.11,    818.69]
    zag[1,*,0] = [   -829.23,   -835.30,   -962.29,   1070.60,   1068.74,   1712.75]
    zag[2,*,0] = [   -453.29,   -445.99,    576.24,    578.64,      0.00,      0.00]
    zag[3,*,0] = [  -1291.72,  -1293.32,   1012.85,   1010.41,      0.00,      0.00]
end

; Positions from early in mission.  These are reference positions used in
; aspect pipeline.

; ACIS-S positions, from obs 66, 678, 4, 12
; ACIS-I positions from obs 65, 1785, 642, 1783
; HRC-S positions  HRC-S-2 is interpolated, rest are from obs 68

if (version eq '1999-Aug') then begin
    yag[0, *, 0] = [  943.267  ,   -752.661 ,     60.5178 ,    2160.88 ,   -1805.61,     409.018] ; ACIS-S
    yag[1, *, 0] = [  940.34,   -753.836,    57.8482,     2160.46,     -1807.96, 406.17] ; ACIS-I
    yag[2, *, 0] = [     -1154.00 ,  1246.33,  -1156.76  ,    1243.97 , 0, 0] ; HRC-S
    yag[3, *, 0] = [     -755.207  ,  856.894,  -1183.96  ,     1287.20 , 0, 0] ; HRC-I

    zag[0, *, 0] = [ -1717.53  ,   -1721.24 ,   -1850.65  ,   187.013  ,   180.713 ,    824.060]
    zag[1, *, 0] = [  -823.16,     -829.470, -956.348,     1076.51,      1074.31, 1718.63]
    zag[2, *, 0] = [     -448.107 ,  -441.52,   581.353  ,    583.802 , 0, 0]
    zag[3, *, 0] = [    -1287.76  ,   -1289.59,   1016.51 ,      1014.77 , 0, 0]
end

if (version eq '1999-Aug-iter1') then begin
    yag[0,*,0] = [    943.23,   -752.66,     60.54,   2160.82,  -1805.57,    409.20]
    yag[1,*,0] = [    940.34,   -753.86,     57.78,   2160.35,  -1807.83,    406.16]
    yag[2,*,0] = [  -1154.07,   1245.80,  -1156.72,   1244.21,      0.00,      0.00]
    yag[3,*,0] = [   -755.19,    857.16,  -1184.06,   1287.22,      0.00,      0.00]
    zag[0,*,0] = [  -1717.40,  -1721.40,  -1850.61,    187.03,    180.79,    824.23]
    zag[1,*,0] = [   -823.24,   -829.56,   -956.48,   1076.56,   1074.36,   1718.65]
    zag[2,*,0] = [   -448.18,   -441.20,    581.31,    583.80,      0.00,      0.00]
    zag[3,*,0] = [  -1287.84,  -1289.34,   1016.64,   1014.67,      0.00,      0.00]
end

if (version eq '1999-Aug-iter2') then begin
    yag[0,*,0] = [    943.23,   -752.66,     60.53,   2160.82,  -1805.57,    409.26]
    yag[1,*,0] = [    940.34,   -753.86,     57.75,   2160.34,  -1807.81,    406.13]
    yag[2,*,0] = [  -1154.06,   1245.67,  -1156.72,   1244.25,      0.00,      0.00]
    yag[3,*,0] = [   -755.21,    857.30,  -1184.07,   1287.22,      0.00,      0.00]
    zag[0,*,0] = [  -1717.31,  -1721.42,  -1850.56,    187.02,    180.79,    824.29]
    zag[1,*,0] = [   -823.26,   -829.57,   -956.49,   1076.57,   1074.37,   1718.64]
    zag[2,*,0] = [   -448.20,   -441.08,    581.32,    583.77,      0.00,      0.00]
    zag[3,*,0] = [  -1287.85,  -1289.24,   1016.65,   1014.64,      0.00,      0.00]
end

if (version eq '1999-Aug-iter3') then begin
    yag[0,*,0] = [    943.25,   -752.66,     60.53,   2160.82,  -1805.57,    409.28]
    yag[1,*,0] = [    940.33,   -753.86,     57.74,   2160.34,  -1807.81,    406.12]
    yag[2,*,0] = [  -1154.05,   1245.63,  -1156.72,   1244.26,      0.00,      0.00]
    yag[3,*,0] = [   -755.22,    857.37,  -1184.07,   1287.22,      0.00,      0.00]
    zag[0,*,0] = [  -1717.26,  -1721.43,  -1850.55,    187.02,    180.79,    824.32]
    zag[1,*,0] = [   -823.27,   -829.57,   -956.48,   1076.57,   1074.37,   1718.63]
    zag[2,*,0] = [   -448.21,   -441.02,    581.32,    583.75,      0.00,      0.00]
    zag[3,*,0] = [  -1287.85,  -1289.19,   1016.65,   1014.63,      0.00,      0.00]
end

if (version eq '1999-Aug-iter4') then begin
    yag[0,*,0] = [    943.27,   -752.66,     60.53,   2160.82,  -1805.57,    409.29]
    yag[1,*,0] = [    940.32,   -753.86,     57.73,   2160.34,  -1807.81,    406.11]
    yag[2,*,0] = [  -1154.05,   1245.63,  -1156.72,   1244.26,      0.00,      0.00]
    yag[3,*,0] = [   -755.23,    857.40,  -1184.07,   1287.22,      0.00,      0.00]
    zag[0,*,0] = [  -1717.23,  -1721.43,  -1850.54,    187.02,    180.79,    824.33]
    zag[1,*,0] = [   -823.28,   -829.57,   -956.48,   1076.57,   1074.37,   1718.63]
    zag[2,*,0] = [   -448.21,   -440.99,    581.32,    583.74,      0.00,      0.00]
    zag[3,*,0] = [  -1287.85,  -1289.17,   1016.65,   1014.63,      0.00,      0.00]
end

; Values from ODB_*_FIDPOS.txt (1999-Sep-4 versions)

if (version eq 'ODB') then begin
    yag[0,*,0] = [  934.000,  -761.000,   52.0000,   2151.00,  -1814.00,   400.000   ]
    yag[1,*,0] = [   937.867,  -758.500,   55.5667,   2155.10,  -1812.30,   402.867   ]
    yag[2,*,0] = [  -1158.60,   1241.83,  -1161.00,   1239.30   , 0.0 , 0.0]
    yag[3,*,0] = [  -760.100,   851.867,  -1188.70,   1281.70   , 0.0 , 0.0]
            
    zag[0,*,0] = [  -1726.00,  -1731.00,  -1860.00,   178.000,   172.000,   815.000]
    zag[1,*,0] = [  -828.500,  -836.700,  -962.000,   1070.20,   1068.60,   1713.30]
    zag[2,*,0] = [  -453.400,  -446.900,   576.100,   578.200  , 0.0 , 0.0]
    zag[3,*,0] = [  -1293.50,  -1293.87,   1012.70,   1010.10  , 0.0 , 0.0]
end

end

;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
function get_obsids, a
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
n_obsid = 1
obsids = [0L]
for i = 0, n_elements(a)-1 do begin
    obsid = a[i].obsid
    ok = 1
    for j = 0, n_elements(obsids)-1 do begin
        if (obsid eq obsids[j]) then ok = 0
    end
    if (ok) then obsids = [obsids, long(obsid)]
end

; return, reverse(obsids[1:*])
return, obsids[1:*]

end

;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;
; Calculate average apparent fid light angles
;
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;

; all = rdb_read('STATS.rdb')
n_all = n_elements(all)

version = 'ODB'
version = '1999-Aug-iter4'
version = '2001-Jul'
version = '2001-Jul-iter'

filename = 'fid_angles_'+version+'.ps'
print, 'Writing ',filename
set_plot,'ps'

device,/color,file=filename

dets = ['ACIS-S', 'ACIS-I', 'HRC-S', 'HRC-I']
!p.charsize=1.6
symsize=0.8

yag = fltarr(4,    $  ; Number of detectors
             6,    $  ; Max number of fids per detector
             1000) $  ; Max number of observations per detector
  - 99999.0           ; Indicator that an entry has no data
zag = yag
sim_z = yag
obsid_arr = yag
sim_z[*,*,0] = 0.0
obsid_arr[*,*,0] = 0.0
y0 = fltarr(4,6)
z0 = fltarr(4,6)

; Set initial values of yag and zag, based on values in UPDATE_2000-Sep/calc_p_lsi.pro
set_initial_values, yag, zag, version

for i_det = 0,3 do begin
    print, 'i_det = ', i_det
    !p.multi = [0,2,3]

    a = get_det(all, dets[i_det])     ; calculate where( all.id_string mat dets[i])
    obsid = get_obsids(a)       ; Get each unique obsid for detector, in reverse order
    x = get_val(i_det, yag)         ; Get the reference X angle (equals yag) for det. i
    y = get_val(i_det, zag)         ; Get the reference Y angle (equals zag) for det. i

    for i_obsid = 0, n_elements(obsid)-1 do begin
        ok = where(a.obsid eq obsid[i_obsid], n_ok)  ; select records for this obsid
        M = fltarr(3,3)
        B = fltarr(3)
        if (n_ok ge 2) then begin             ; skip unless there are at least 2
            xr = a[ok].ang_y_med
            yr = a[ok].ang_z_med
            id = intarr(n_ok)-1 ; Force a crash unless following loop succeeds
            for i_dat = 0, n_ok-1 do begin    ; Loop over fids for this obsid
                for i_id = 1, 6 do begin      ; Find the fid id number (0-5 ACIS, 0-3 HRC)
                    if (strpos(a[ok[i_dat]].id_string, strtrim(string(i_id),2)) ne -1) then id[i_dat] = i_id-1
                end

                xi  = x[id[i_dat]]
                yi  = y[id[i_dat]]
                xri = xr[i_dat]
                yri = yr[i_dat]

                M = M + [[ 1   ,     0    , -yri            ], $
                         [ 0   ,     1    ,  xri            ], $
                         [ -yri,   xri    , xri*xi + yri*yi ]]

                B = B +  [ xi - xri, $
                           yi - yri, $
                           xri*yi - yri*xi ]

            end 

            vals = invert(M) ## B
            print, 'obsid = ', obsid[i_obsid], 'dy = ',vals[0], ' dz  = ', vals[1], ' sim_z = ', a[ok[0]].sim_z_offset, $
              ' date_obs = ',a[ok[0]].date_obs

            ; Put the translated, rotated values back into the main array of fid angles
            for i_dat = 0, n_ok-1 do begin
                dx = vals[0]
                dy = vals[1]
                dt = vals[2]
                yag[i_det, id[i_dat], i_obsid+1] = cos(dt) * xr[i_dat] - sin(dt) * yr[i_dat] + dx
                zag[i_det, id[i_dat], i_obsid+1] = sin(dt) * xr[i_dat] + cos(dt) * yr[i_dat] + dy
                sim_z[i_det, id[i_dat], i_obsid+2] = a[ok[i_dat]].sim_z_offset
                obsid_arr[i_det, id[i_dat], i_obsid+2] = a[ok[i_dat]].obsid
            end
        end 
    end

    ; Make the plots

    for i_id = 0,5 do begin
        y_ang = get_ang_array(i_det, i_id, yag)
        z_ang = get_ang_array(i_det, i_id, zag)
        sim_z_off = get_ang_array(i_det, i_id, sim_z)
        if (n_elements(y_ang) gt 2) then begin
            ; Select only the new data (reject original starting point)
            y_ang = y_ang[1:*]  
            z_ang = z_ang[1:*]
            sim_z_off = sim_z_off[1:*]

            ; Subtract off the mean and calculate plot range
            y0[i_det,i_id] = mean(y_ang[1:*])
            z0[i_det,i_id] = mean(z_ang[1:*])

            y_ang_orig = y_ang[0]-y0[i_det,i_id]
            z_ang_orig = z_ang[0]-z0[i_det,i_id]

            y_ang = y_ang - y0[i_det,i_id]
            z_ang = z_ang - z0[i_det,i_id]
            dy = max(y_ang) - min(y_ang)
            dz = max(y_ang) - min(y_ang)
            dd = max([dy,dz,1.0])/2.0
            yr = (min(y_ang)+ max(y_ang))/2.0 + [-dd,dd]
            zr = (min(z_ang)+ max(z_ang))/2.0 + [-dd,dd]

            ; Make a title string with fid angle
            title_string = dets[i_det] + '-' + strtrim(i_id+1,2) + $
              ' Y='+string(y0[i_det,i_id],format='(f8.2)') + $
              ' Z='+string(z0[i_det,i_id],format='(f8.2)')

            ; Plot the bounding box
            plot, y_ang, z_ang, /nodata, color=color_black, $
              title=title_string, xrange=yr, yrange=zr
            oplot, y_ang, z_ang, psym=7, color=color_blue ; All points
            ok = where(abs(sim_z_off-median(sim_z_off)) ge 20.0, n_ok)
            if (n_ok ge 1) then oplot, y_ang[ok], z_ang[ok], psym=6, color=color_red
            oplot, [y_ang_orig], [z_ang_orig], $
              psym=1, symsize=8, color=color_magenta
        end
    end
end 

device,/close
set_plot,'x'

for i_det=0, 3 do begin
    print, 'yag[',i_det,',*,0] = [', $
      y0[i_det,0], ',', $
      y0[i_det,1], ',', $
      y0[i_det,2], ',', $
      y0[i_det,3], ',', $
      y0[i_det,4], ',', $
      y0[i_det,5], ']', $
      format='(a,i1,a,6(f10.2,a))'
end
for i_det=0, 3 do begin
    print, 'zag[',i_det,',*,0] = [', $
      z0[i_det,0], ',', $
      z0[i_det,1], ',', $
      z0[i_det,2], ',', $
      z0[i_det,3], ',', $
      z0[i_det,4], ',', $
      z0[i_det,5], ']', $
      format='(a,i1,a,6(f10.2,a))'
end

end