;+
; NAME:
;        sb_pix23d
; PURPOSE:
;        calculates 3d coord. from pixel coord. in two images
; CATEGORY:
;        image processing
; CALLING SEQUENCE:
;        rval = sb_3d2pix(pix1,pix2,wcs1=wcs1,wcs2=wcs2)
; INPUTS:
;        pix1 : fltarr(2,n). Pixel coordinates in 1st view
;               Can use multiple coord (n gt 1)
;        pix2 : fltarr(2,n). Pixel coordinates in 2nd view
;               Note: pix2[1,*] (i.e.,pix2_y) is ignored in calculating rval!
;        wcs1 = WCS structure for 1st view (applies to all n coord. sets)
;        wcs2 = WCS structure for 2nd view (applies to all n coord. sets)
; KEYWORDS (INPUT):
;        /carr  : output reports carrington instead of Stonyhurst latitude
; OUTPUTS:
;        rval = fltarr(3,n). heliographic coordinates [lon,lat,rad]
;               lon,lat: Stonyhurst (earth centered) or Carrington (if /carr).
;               rad: in units of solar radius
; KEYWORDS (OUTPUT):
; COMMON BLOCKS:
; SIDE EFFECTS:
; RESTRICTIONS:
; PROCEDURE:
; MODIFICATION HISTORY:
;        JPW, 10/30/2007
;-

FUNCTION sb_pix23d,px1,px2,wcs1=wcs1,wcs2=wcs2,carr=carr

n1 = size(px1)
n2 = size(px2)
if n1[0] gt 1 then n1=n1[2] else n1=1L
if n2[0] gt 1 then n2=n2[2] else n2=1L
if n1 ne n2 then return,-1   ; error

solrad = 6.9599d8

; 1. calculate 3D ray for each point in px1
; (in heliocentric cartesian coordinates, z towards Earth (or Carr. meridian))

; 1.1 coord. of observer:
od1 = [0d0,0d0,wcs1.position.dsun_obs/solrad]

; 1.2 coord. of pixel, assuming it is in plane through sun center,
;     perpendicular to direction observer to sun center
;     wcs_get_coord provides x/y coordinates in arcsec, rescale to solrad, z=0
d1 = dblarr(3,n1)
a2s = wcs1.position.dsun_obs*!dpi/(solrad*1.8d2*3.6d3)  ; arcsec2solrad
d1[0,0] = wcs_get_coord(wcs1,px1) * a2s

; 1.3 rotate to solar equator (B0 rotation)
b0 = wcs1.position.solar_b0 / !radeg
d0 = od1
od1[1] = d0[1] * cos(b0) + d0[2] * sin(b0)
od1[2] = d0[2] * cos(b0) - d0[1] * sin(b0)
d0 = d1
d1[1,0] = d0[1,*] * cos(b0) + d0[2,*] * sin(b0)
d1[2,0] = d0[2,*] * cos(b0) - d0[1,*] * sin(b0)

; 1.4 rotate to earth longitude (or carrington meridian)
a0 = -wcs1.position.hgln_obs / !radeg
if keyword_set(carr) then a0 = a0 - wcs1.position.carr_earth / !radeg
d0 = od1
od1[2] = d0[2] * cos(a0) + d0[0] * sin(a0)
od1[0] = d0[0] * cos(a0) - d0[2] * sin(a0)
d0 = d1
d1[2,0] = d0[2,*] * cos(a0) + d0[0,*] * sin(a0)
d1[0,0] = d0[0,*] * cos(a0) - d0[2,*] * sin(a0)

; - ray is pixel_vector plus q1*(obs_vector-pixel_vector): d1 + q1*(od1-d1)

; 2. do step 1. for px2, obs_vector2, and px2b(x=px2_x,y=px2_y+1000)
; 2.1 coord. of observer:
od2 = [0d0,0d0,wcs2.position.dsun_obs/solrad]

; 2.2 coord. of pixel, assuming it is in plane through sun center,
;    perpendicular to direction observer to sun center
;    wcs_get_coord provides x/y coordinates in arcsec, rescale to solrad, z=0
d2 = dblarr(3,n1)
a2s = wcs2.position.dsun_obs*!dpi/(solrad*1.8d2*3.6d3)  ; arcsec2solrad
d2[0,0] = wcs_get_coord(wcs2,px2) * a2s

; 2.3 coord. of pixel 1000 pixel units above px2 (i.e., same x as px2)
d2b = dblarr(3,n1)
px2b = px2
px2b[1,0] = px2b[1,*]+1d3
d2b[0,0] = wcs_get_coord(wcs2,px2b) * a2s

; 2.4 rotate to solar equator (B0 rotation)
b0 = wcs2.position.solar_b0 / !radeg
d0 = od2
od2[1] = d0[1] * cos(b0) + d0[2] * sin(b0)
od2[2] = d0[2] * cos(b0) - d0[1] * sin(b0)
d0 = d2
d2[1,0] = d0[1,*] * cos(b0) + d0[2,*] * sin(b0)
d2[2,0] = d0[2,*] * cos(b0) - d0[1,*] * sin(b0)
d0 = d2b
d2b[1,0] = d0[1,*] * cos(b0) + d0[2,*] * sin(b0)
d2b[2,0] = d0[2,*] * cos(b0) - d0[1,*] * sin(b0)

; 2.5 rotate to earth longitude (or carrington meridian)
a0 = -wcs2.position.hgln_obs / !radeg
if keyword_set(carr) then a0 = a0 - wcs2.position.carr_earth / !radeg
d0 = od2
od2[2] = d0[2] * cos(a0) + d0[0] * sin(a0)
od2[0] = d0[0] * cos(a0) - d0[2] * sin(a0)
d0 = d2
d2[2,0] = d0[2,*] * cos(a0) + d0[0,*] * sin(a0)
d2[0,0] = d0[0,*] * cos(a0) - d0[2,*] * sin(a0)
d0 = d2b
d2b[2,0] = d0[2,*] * cos(a0) + d0[0,*] * sin(a0)
d2b[0,0] = d0[0,*] * cos(a0) - d0[2,*] * sin(a0)

; - plane is pixel_vector2 plus q2*(obs_vector2-pixel_vector2) plus
;                          plus q3*(pixel_vector2b-pixel_vector2)
;   i.e.: d2 + q2*(od2-d2) + q3*(d2b-d2)

; 3. find intersection of ray with plane.  Set to origin if no solution
;    i.e., solve d1 + q1*(od1-d1) = d2 + q2*(od2-d2) + q3*(d2b-d2)
xyz = dblarr(3,n1)
for j=0L,n1-1 do begin         ; need to solve equations for each point
  a = transpose([[od1-d1[*,j]],[od2-d2[*,j]],[d2b[*,j]-d2[*,j]]])
  b = d2[*,j]-d1[*,j]
  svdc,a,w,u,v
  q = svsol(u,w,v,b)
  xyz[0,j] = d1[*,j] + q[0]*(od1-d1[*,j])
endfor

; 4. convert cartesians to rval (lon,lat,rad)
rval = fltarr(3,n1)
rr = sqrt(total(xyz*xyz,1))
ww = where(rr gt 0d0,nww)
if nww gt 0 then begin
  rxz = sqrt(xyz[0,ww]*xyz[0,ww]+xyz[2,ww]*xyz[2,ww])
  rval[0,ww] = !radeg*atan(xyz[0,ww],xyz[2,ww])
  rval[1,ww] = !radeg*atan(xyz[1,ww],rxz)
  rval[2,ww] = rr[ww]
endif

return,rval

end