Function iris_solar2wedge_tl,xtarget,ytarget,xoff=xoff,yoff=yoff,roll_angle=roll_angle,xypt=xypt, $
  closest=closest,nsearch=nsearch,short=short,m1i=m1i,m2i=m2i
;+
; Name    : iris_solar2wedge_tl
;
; Purpose :  Compute wedge motor positions for a given solar (x,y);
;               version used in IRIS timeline program
;
; Use     : IDL> wm12 = iris_solar2wedge_tl(xtarget,ytarget)
;
; Input   :  xtarget, ytarget = desired solar coordinates in arcseconds
;                x = EW, East is positive, y = NS, North is positive
;
; Output  : [M1,M2] = motor positions in steps for Wedge Motor1 & WM2
;               wedge motor positions are integers in range 0-239, 1.5 degree steps
;
; Keyword Inputs: 
;    xoff = offset in X of Science Telescope from Guide Telescope [103]
;    yoff = offset in Y of Science Telescope from Guide Telescope [-82]
;    roll_angle = IRIS roll angle, rotation of S/C about +Z axis using right-hand rule, degrees
;         roll_angle small & positive moves a pointing on the N central meridian (0,+y) 
;         into the NW quadrant of the Sun (+x',+y') [default = 0.]
;    closest:  if set, search over range of +/-nsearch (default=3) to find
;         the closest match in arcsec, since the analytic formula is only
;         approximate when the wedges are not identical [default = 1]
;    nsearch = half-width of search for case /closest [3]
;    short:  if set, use the solution that requires the shorter slew from
;         the initial positions m1i, m2i, of the 2 degenerate
;         solutions to the analytical model; [default = 0]
;         don't set closest if you set short--closest will be ignored       
;    m1i, m2i - initial wedge positions [120, 120]
;
; Keyword Output:
;    xypt = [x,y] coordinates that wedges actually point to; may be
;        off by as much as 10 arcsec from xtarget or ytarget
;
; Calls:  iris_wedge2solar_tl, iris_slew_time6 (only if /short)
;
; Examples
;    wms = iris_solar2wedge_tl(xtarget,ytarget,/close,nsearch=6,xypt=xypt,xoff=103,yoff=-80,roll_ang=-45.)
;    wms = iris_solar2wedge_tl(xtarget,ytarget,/short,m1i=77,m2i=132,xypt=xypt)
;  
; TD Tarbell  Version 1.0  3-Dec-2012
; TDT         Version 2.0   1-Jul-2013  sign change of X & Y, discovered by
;                    J-P, agreed by TDT, verified in flight 30 June
; TDT         Version 3.0 28-Aug-2013  remove changing x,y; add default offsets
; TDT         Version 3.1    23-Jun-2014  corrected offsets, cleaned up 
;-

x = xtarget
y = ytarget
x0 = x
y0 = y
if (keyword_set(roll_angle)) then begin
   th = 2.*!pi*roll_angle/360.
   sth = sin(th)
   cth = cos(th)
   x =  x0*cth - y0*sth
   y =  x0*sth + y0*cth
end else roll_angle = 0.

; These offsets are from 4-limb coalignments in Nov 2013 & 2014 and
;    are the current values in timeline
; Set these keywords to 0.001 if you want to use 0.
if not keyword_set(xoff) then xoff = 103.
if not keyword_set(yoff) then yoff = -82.

x = x - xoff
y = y - yoff

; these parameters need to be consistent with those in iris_wedge2solar_tl.pro
rmax = 0.5*(634.8+628.5)
offset2 = 113.57
m1v = 217.875
fac = 2.*!pi/240.

rtot = sqrt(x^2+y^2)
if (rtot ge 2.*rmax) then begin
   print,'Fatal error--target outside range of wedges,',x0,y0,rtot
   return,-1
end

; The following inverts the function for offsets by the wedges 


; *************************************************
;xx = x/rmax
;yy = y/rmax
; Here is the sign change for Version 2.0
xx = -x/rmax
yy = -y/rmax
; *************************************************


dth = acos((xx^2+yy^2)/2.-1.)/fac
dm = round(dth-120.+offset2)    ; m2-m1
ath = atan(xx,-yy)/fac + m1v

;print,'dm,ath ',dm,ath
   
; atan(a,b) =  angle theta between -pi and pi for which a=r*sin(theta)
;   and b=r*cos(theta) for some positive r, 
;   when at least one of a,b is NE 0, and
;  atan(0.,0.) = 0. for that special case
; This is usually called as atan(y,x) = arctan(y/x) when both > 0

m1 = round(-dth/2. + ath)
m2 = m1+round(dm)

if not keyword_set(closest) then closest = 0
if keyword_set(short) then begin
   closest = 0    ; don't do closest if you're doing short
   if (n_elements(m1i) eq 0) then m1i = 120    ; need initial positions
   if (n_elements(m2i) eq 0) then m2i = 120
; Make up destinations that ensure motion goes shortest way around
; mm1f & mm2f may be < 0 or > 239;  this is OK for counting hops
   if (m1 gt m1i+120) then mm1f = m1-240 else if (m1 lt m1i-120) then mm1f=m1+240 else mm1f=m1
   if (m2 gt m2i+120) then mm2f = m2-240 else if (m2 lt m2i-120) then mm2f=m2+240 else mm2f=m2
   tm1m2 = iris_slew_time6(m1i,m2i,mm1f,mm2f)

; iris_wedge2solar_tl(i,j) ~=  iris_wedge2solar_tl(j+6,i-6)
; This would be exact for identical wedges, and in reality can be
;  off but up to 20" (before searching) 
   w1 = m2+6
   w2 = m1-6
   if (w1 gt m1i+120) then mm1f = w1-240 else if (w1 lt m1i-120) then mm1f=w1+240 else mm1f=w1
   if (w2 gt m2i+120) then mm2f = w2-240 else if (w2 lt m2i-120) then mm2f=w2+240 else mm2f=w2
   tw1w2 = iris_slew_time6(m1i,m2i,mm1f,mm2f)
   if (tw1w2 lt tm1m2) then begin
      m1 = w1
      m2 = w2
   end
end

if not keyword_set(nsearch) then nsearch = 3
minerr = 1.e10
for i=-nsearch,nsearch do begin
for j=-nsearch,nsearch do begin
   w1 = m1+i
   w2 = m2+j
   xx = iris_wedge2solar_tl(w1,w2,xoff=xoff,yoff=yoff,roll_angle=roll_angle)
   miss = (x0-xx(0))^2 +  (y0-xx(1))^2
   if (miss lt minerr) then begin
      minerr = miss
      imin = i
      jmin = j
   end
end
end
m1 = m1+imin
m2 = m2+jmin

if (closest) then begin
   minerr0 = minerr
   mm1 = m2+6
   mm2 = m1-6
   for i=-nsearch,nsearch do begin
      for j=-nsearch,nsearch do begin
         w1 = mm1+i
         w2 = mm2+j
         xx = iris_wedge2solar_tl(w1,w2,xoff=xoff,yoff=yoff,roll_angle=roll_angle)
         miss = (x0-xx(0))^2 +  (y0-xx(1))^2
         if (miss lt minerr) then begin
            minerr = miss
            imin = i
            jmin = j
         end
      end
   end
   if (minerr lt minerr0) then begin
      m1 = mm1+imin
      m2 = mm2+jmin
   end
end

if (m1 lt 0) then m1 = m1+240
m1 = m1 MOD 240
if (m2 lt 0) then m2 = m2+240
m2 = m2 MOD 240

;print,x0,y0,transpose(iris_wedge2solar_tl(m1,m2)),m1,m2
xypt = iris_wedge2solar_tl(m1,m2,xoff=xoff,yoff=yoff,roll_angle=roll_angle)

return, [m1,m2]
end