; Fit new values of ACA plate scale based on on-orbit star data
;
; Tom Aldcroft,  Jan 2002

;;***************************************************************************
function chisq, p
;;***************************************************************************
;;  Calculate predicted yag,zag based on flight calibration values
;; 
common chisq_data, ycc, zcc, vt1, vt2, p_map, sp, t

d2r = !pi/180.0d0

nv=n_elements(sp)
n_coeff=n_elements(zcc)
n_p = n_elements(p)

; Make local versions of coefficients
ycc_l = ycc
zcc_l = zcc

; The first half of the 'p' array has a mapped set of coefficients
; for ycc, then second half has coefficients for zcc

; print, 'ycc',ycc_l, 'p_map',p_map, 'p',p, 'np',n_p

ycc_l[p_map] = p[0     : n_p/2-1]
zcc_l[p_map] = p[n_p/2 : n_p-1]

y=total(vt1*rebin(ycc_l,n_coeff,nv,/samp),1)
z=total(vt1*rebin(zcc_l,n_coeff,nv,/samp),1)

if nv gt 1 then begin
  y1=reform(y)/3600.
  z1=reform(z)/3600.
endif

y=total(vt2*rebin(ycc_l,n_coeff,nv,/samp),1)
z=total(vt2*rebin(zcc_l,n_coeff,nv,/samp),1)

if nv gt 1 then begin
  y2=reform(y)/3600.
  z2=reform(z)/3600.
endif

T = [[1d0, 0d0, 0d0], $
     [0d0, 1d0, 0d0], $
     [0d0, 0d0, 1d0]]

chisq = 0.0
yz_dist = dblarr(nv)

yagzag_to_radec, y1, z1, T, ra1, dec1
yagzag_to_radec, y2, z2, T, ra2, dec2
yz_dist = acos( cos(dec1*d2r)*cos(dec2*d2r) * cos((ra1-ra2)*d2r) + $
                sin(dec1*d2r)*sin(dec2*d2r)) / d2r * 3600

dx = yz_dist - sp.dist
dx_mean = mean(dx)
rms = sqrt(total((dx - dx_mean)^2)/nv)
ok = where(abs(dx - dx_mean) lt rms*3.0)
nok = where(abs(dx - dx_mean) gt rms*3.0, n_nok)

chisq = total(dx[ok]^2)
plot, yz_dist[ok], dx[ok], psym=3, yrange=[-2,2]
if (n_nok gt 0) then oplot, yz_dist[nok], dx[nok], psym=4
print, 'chisq = ', chisq
print, [transpose(ycc_l), transpose(zcc_l)], format='(2e)'

return, chisq

end 

;;***************************************************************************
;;  Read in the plate scale records containing star data and centroids
;;  and constuct records for each star pair in each obsid
;;***************************************************************************
pro fit_ps, sp_out

common chisq_data, ycc, zcc, vt1, vt2, p_map, sp, t
d2r = !pi/180.0d0

; Read in ps = plate scale records
if (n_elements(ps) eq 0) then ps = mrdfits('clean_plate_scale.fits',1); rarread, 'plate_scale.dat', ps

ps = [ps, ps[0]]   ; Add a bogus final element to force processing of last obsid
n_ps = n_elements(ps)

; Set up a pseudo gsprops structure for each observation
obsid = ps[0].obsid
n_gs = 0
gs = ps[0:7]

; Set up the final sp = Star Pair record structure
sp = replicate({obsid:0l, dist:0.0, cen_i1:0.0, cen_j1:0.0, cen_i2:0.0, cen_j2:0.0}, n_ps*4)
n_sp = 0

for p = 0, n_ps-1 do begin
    if (ps[p].obsid ne obsid) then begin
       ; Make the set of star pairs
        for i = 0, n_gs-2 do begin
            for j = i+1, n_gs-1 do begin
                dec1  = gs[i].dec
                ra1   = gs[i].ra
                dec2  = gs[j].dec
                ra2   = gs[j].ra
                sp[n_sp].dist  =  acos( cos(dec1*d2r)*cos(dec2*d2r) * cos((ra1-ra2)*d2r) + $
                                        sin(dec1*d2r)*sin(dec2*d2r)) / d2r * 3600
                sp[n_sp].cen_i1 = gs[i].cen_i
                sp[n_sp].cen_j1 = gs[i].cen_j
                sp[n_sp].cen_i2 = gs[j].cen_i
                sp[n_sp].cen_j2 = gs[j].cen_j
                sp[n_sp].obsid = gs[j].obsid
                n_sp = n_sp + 1
            end
        end

        n_gs = 0
        obsid = ps[p].obsid
    end
    gs[n_gs] = ps[p]
    n_gs = n_gs + 1
end

sp = sp[0:n_sp-1]

;;***************************************************************************
;;  Set up and perform the actual minimization to determine PS coefficients
;;***************************************************************************

; Read in the flight (eeprom) plate scale coefficients

if (n_elements(rbal) eq 0) then begin
;    rrdb, 'eeprom_cal.rdb', rbal
;    rrdb, 'dist_cal.rdb', rbal
    rarread, 'dist_cal.dat', rbal
    rbal = rbal(0:18)             ; drop step parameter in Ball calibration file (which is 0 anyway)
    ycc  = rbal.pristaryag
    zcc  = rbal.pristarzag
end

ccd_temp = 14.0
t = dblarr(n_sp) +  ccd_temp
r = sp.cen_i1
c = sp.cen_j1

nv=n_elements(c)
np=n_elements(zcc)

v=[replicate(1.,nv),c,r,t,c*c,c*r,c*t,r*r,r*t,t*t,$
   c*c*c,c*c*r,c*c*t,c*r*r,c*r*t,c*t*t,r*r*r,r*r*t,r*t*t]
vt1=transpose(reform(v,nv,np))

r = sp.cen_i2
c = sp.cen_j2

v=[replicate(1.,nv),c,r,t,c*c,c*r,c*t,r*r,r*t,t*t,$
   c*c*c,c*c*r,c*c*t,c*r*r,c*r*t,c*t*t,r*r*r,r*r*t,r*t*t]
vt2=transpose(reform(v,nv,np))

y_dir = set_y_dir(1)
z_dir = set_z_dir(1)

p_map = [1,2,4,5,7,10,11,13,16]; [
n_p_map = n_elements(p_map)
p = [ycc[p_map], zcc[p_map]]
dir = rebin([y_dir[p_map], z_dir[p_map]], 2*n_p_map, 2*n_p_map, /samp) * identity(2*n_p_map)
scale = [y_dir[p_map], z_dir[p_map]]
ftol = 1e-4
last_p = p*0.0

powell, p, dir , ftol, chi_min, 'chisq', iter=n_iter

; p = amoeba(ftol, function_name='chisq', scale=dir, p0=p)

print, 'After ', n_iter,' iterations got a min of ',chi_min
print, '  at parameter values ', p
print, '  Compare to originals: ', [ycc[p_map], zcc[p_map]]

end

function set_y_dir, dummy
dir = [ 0.0,     $
   0.0010319214, $
  -0.0010186732, $
      0.0000000, $
 -6.2672140e-06, $
 -7.2170886e-06, $
  6.7236841e-05, $
 -1.1880052e-05, $
  0.00012582943, $
      0.0000000, $
  6.5150076e-09, $
  2.7592870e-08, $
  6.9751904e-07, $
 -3.5474217e-08, $
 -5.2462428e-07, $
 -2.1142854e-05, $
  1.1839438e-08, $
 -8.3415261e-07, $
  8.7049206e-06]

return, dir
end

function set_z_dir, dummy
dir= [ 0.0,       $
   0.00076551976, $
    0.0042026182, $
       0.0000000, $
   7.1356575e-06, $
   8.6520504e-06, $
  -0.00019935229, $
   6.2117226e-06, $
  -5.7805501e-05, $
       0.0000000, $
  -1.6071289e-08, $
  -7.4567873e-09, $
  -3.7932019e-07, $
  -3.3945380e-08, $
  -3.5165436e-07, $
  -1.2513268e-05, $
  -7.0430104e-09, $
   4.2083780e-07, $
   2.4442798e-05]

return, dir
end