;+
; Project     :	SOHO - CDS
;
; Name        :	GET_OPS_POS
;
; Purpose     :	Calculates the CDS OPS positions for a given solar (x,y).
;
; Explanation :	See Notes on pointing calculation.
;               Now uses corrected second order calculation.
;
; Use         : <get_ops_pos, solarx, solary, opsl, opsr, ERRMSG=errmsg>
;
; Inputs      : solarx = solar x co-ordinate in arcsecs;
;               solary = solar y co-ordinate in arcsecs.
;
; Opt. Inputs : None.
;
; Outputs     : opsl   = OPS left  position;
;               opsr   = OPS right position.
;
; Opt. Outputs:	None.
;
; Keywords    : None.
;       ERRMSG    = If defined and passed, then any error messages will be
;                   returned to the user in this parameter rather than
;                   depending on the MESSAGE routine in IDL.  If no errors are
;                   encountered, then a null string is returned.  In order to
;                   use this feature, ERRMSG must be defined first, e.g.
;
;                       ERRMSG = ''
;                       GET_OPS_POS, ERRMSG=ERRMSG, ... 
;                       IF ERRMSG NE '' THEN ...
;
;       SC_ROLL   = The SOHO roll angle in degrees.  If not specified
;       then the procedure will get this information by calling GT_SC_POINT()  
;
;       UTC       = The time at which to calculate the correct OPS  position.  This is 
;                   necessary because the S/C roll angle will change over time.  If this
;                   is not specified, then it will default to time when the procedure
;                   is being run.
;
; Calls       :	ops_point, cp_get_entry.
;                
; Common      :	None.
;
; Restrictions:	None.
;
; Side effects:	None.
;
; Category    :	Command preparation.
;
; Prev. Hist. :	None.
;
; Written     :	Version 0.00, Martin Carter, RAL, 14/10/94
;
; Modified    :	Version 0.01, Martin Carter, RAL, 28/11/94
;                            Added proforma.
;               Version 0.1, MKC, 11/8/95
;                            Added interrogation of CDHS state database for pointing parameters.
;                            Converted to full pointing calculation.
;		Version 0.2, MKC, 28/9/95
;			Added keyword ERRMSG
;               Version 0.3, MKC, 7/11/95
;                       Used cp_get_entry.
;               Version 0.4, MKC, 23/11/95
;                       Modified ops calculation to try to agree better with SC x,y calculation
;               Version 0.5, MKC, 5/1/96
;                       Modified state database storage of cb2point values to reflect proper command
;                       format so have to change handling of indeces.
;               Version 0.6, MKC, 20/3/96
;                       Removed part of calculation which takes into account a possible difference between the
;                       OPS left and right corresponding to the sun centre and the central position of the actuators
;                       about which the OPS movement is calculated. This allows better agreement between the 
;                       calculated on board position and the required position.
;               Version 0.7 Added common block for speed.  CDP, 1-May-96
;		Version 8, William Thompson, GSFC, 12-Jun-1997
;			Let input parameters be arrays.
;               Version 9, William Thompson, GSFC, 23-Jun-2003
;                       Look at SOHO_ORIENT environment variable
;               Version 10, Ronald Yurow, GSFC, 15-Nov-2010
;                       Modified to work with SOHO pointed at ecliptic North.
;
; Version     :	Version 10, 15-Nov-2010
;-
;**********************************************************

PRO get_ops_pos, solar_x, solar_y, opsl, opsr, UTC = utc,  SC_ROLL = scroll, ERRMSG=ERRMSG
;
;  common block to prevent repetitive database access
;
common lrxy, ss

;  This section is no longer will be needed.
;
;  If the enviroment variable SOHO_ORIENT is 180, then invert the input
;  parameters.
;
;solarx = solar_x
;solary = solar_y
;if getenv('SOHO_ORIENT') eq 180 then begin
;    solarx = -solarx
;    solary = -solary
;endif

; Check if UTC keyword was set.  If it was not, then we will set it
; to the current time.

IF NOT KEYWORD_SET (utc) THEN GET_UTC, utc

; Check if a S/C roll was specified as parameter.  If it was,
; then use that.  Otherwise, get the S/C roll for the time
; specified in utc.

IF N_ELEMENTS (scroll) EQ 0  THEN scroll = (get_sc_point(utc)).sc_roll

; Convert to radians.
    
scroll = scroll * !dtor

; Adjust solarx and solary to account for the tilt of SOHO relative to 
; Solar N.

c_roll_in = cos(scroll)
s_roll_in = sin(scroll)

solarx = solar_x * c_roll_in - solar_y * s_roll_in
solary = solar_x * s_roll_in + solar_y * c_roll_in

  ; get ops pointing parameters
if n_elements(ss) eq 0 then begin
  ss = cp_get_entry ( 'CB2POINT', [0,2,4,6,7,8,10], ERRMSG=ERRMSG )
endif

  ; check ok

  IF N_ELEMENTS(ERRMSG) NE 0 THEN IF ERRMSG NE '' THEN GOTO, HANDLE_ERROR

  ; constants
; NB values stored as 16 bit values for cb2point command
;    only OPS R may be bigger than 65536

  ops_l_zero = ss(0).active                            ; ops l position corresponding to sun centre
  ops_r_zero = ss(1).active                            ; ops r position corresponding to sun centre
  ops_K      = ss(2).active                            ; ops K value
  ops_R      = ss(3).active + ISHFT(ss(4).active,16)   ; ops R value
  ops_LTHE   = ss(5).active                            ; ops L theta value
  ops_LPHI   = ss(6).active                            ; ops L phi value

  ; assume that the OPS left and right corresponding to the sun centre 
  ; are at the central position of the OPS about which the calculation is valid

  ; set up some constants
  ; make these double precision to force rest of calculation to be double precision

  dxlphirk = solarx*ops_LPHI/DOUBLE(ops_K*ops_R)
  dyltherk = solary*ops_LTHE/DOUBLE(ops_K*ops_R)

  ; ops first order approximation 

  ;  opsl  = ops_l_zero - ( dx*ops_LPHI + dy*ops_LTHE)/(2*ops_K)
  ;  opsr  = ops_r_zero + ( dx*ops_LPHI - dy*ops_LTHE)/(2*ops_K)

  ; ops second order approximation

  ;  opsl = ops_l_zero + ops_R/2.0 * (-dxlphirk-dyltherk+0.5*dxlphirk^2+0.5*dyltherk^2-dxlphirk*dyltherk)

  ;  opsr = ops_r_zero + ops_R/2.0 * ( dxlphirk-dyltherk+0.5*dxlphirk^2+0.5*dyltherk^2+dxlphirk*dyltherk)

  ; use full approximation to get ops relative to central position

  opsl = ops_l_zero - ops_R/2.0 * ( 1 - SQRT(1.D0 + 2*(-dxlphirk - dyltherk + dxlphirk^2 + dyltherk^2)) )

  opsr = ops_r_zero - ops_R/2.0 * ( 1 - SQRT(1.D0 + 2*( dxlphirk - dyltherk + dxlphirk^2 + dyltherk^2)) )
 
  ; Convert OPS values to nearest integer values

  opsl = ROUND(opsl)
  opsr = ROUND(opsr)

  ; calculate corresponding solar x and y using on-board calculation

  dm = opsl - opsr - ( ops_l_zero - ops_r_zero )

  dp = opsl + opsr - ( ops_l_zero + ops_r_zero )

  sc_solarx = - ( ops_K * ( dm + (dm*dp)/ops_R ) ) / ops_LPHI
  sc_solary = - ( ops_K * ( dp - (dm^2 + dp^2)/(2*ops_R) ) ) / ops_LTHE

  ; use first order approximation to correct to SC values

  dx = sc_solarx - solarx
  dy = sc_solary - solary

  opsl  = opsl - ( dx*ops_LPHI + dy*ops_LTHE)/(2*ops_K)
  opsr  = opsr + ( dx*ops_LPHI - dy*ops_LTHE)/(2*ops_K)

  ; Convert OPS values to nearest integer values

  opsl = FIX(ROUND(opsl))
  opsr = FIX(ROUND(opsr))

  ; test ops values

  opsl_max = max(opsl, min=opsl_min)
  opsr_max = max(opsr, min=opsr_min)
  invalid = 0
  IF opsl_max GT 4095 then begin
	w = where(opsl eq opsl_max)
	opsl_bad = opsl_max
	opsr_bad = opsr(w(0))
	invalid = 1
  end else if opsl_min LT 0 then begin
	w = where(opsl eq opsl_min)
	opsl_bad = opsl_min
	opsr_bad = opsr(w(0))
	invalid = 1
  end else if opsr_max GT 4095 then begin
	w = where(opsr eq opsr_max)
	opsl_bad = opsl(w(0))
	opsr_bad = opsr_max
	invalid = 1
  end else if opsr_min LT 0 THEN BEGIN
	w = where(opsr eq opsr_max)
	opsl_bad = opsl(w(0))
	opsr_bad = opsr_max
	invalid = 1
  endif

  IF INVALID THEN BEGIN
    IF N_ELEMENTS(ERRMSG) NE 0 THEN BEGIN
      ERRMSG = 'INVALID OPS VALUES : ' + STRTRIM ( opsl_bad, 1) + ', ' + $
		STRTRIM ( opsr_bad, 1)
      GOTO, HANDLE_ERROR
    ENDIF ELSE $
      MESSAGE, 'INVALID OPS VALUES : ' + STRTRIM ( opsl_bad, 1) + ', ' + $
		STRTRIM ( opsr_bad, 1)
  ENDIF

  RETURN
  ;
  ;  Error handling point.
  ;
  HANDLE_ERROR:
  
  ERRMSG = 'GET_OPS_POS: ' + ERRMSG

END
