; docformat = 'rst'
;+
;  Where there are housekeeping gaps in the records used to generate the
;    quaternions, try to interpolate the record to fill them.
;  If there are no gaps return the quaternion array unchanged
;
; :Author:
;   Bill Barrett
;
; :Params:
;   usec : in, required, type=double
;     Input GPS time in microseconds. Must be a scalar.
;   updated_usec: out, type=double
;     If there are no gaps in the data this will the same as the
;       input array. If there are gaps, then this will be the
;       input array plus the interpolated times.
;   value : in / out, required, type=double[n_elements(usec), 4]
;     Two dimensional for each of four pre-quaternion components for
;       each time in the usec array. This array will be changed
;       if there are gaps to be interpolated over.
;
; :Examples:
;   interpolate_quat_if_necessary, original_usec, usec, (*qq[i]).eu
;
;-
pro interpolate_quat_if_necessary, usec, updated_usec, value ;, index=index , plot_data=plot_data

  ; Are there gaps in the input data that need to be filled?
  updated_usec = [usec[0]]
  j = 0
  for i = 1, n_elements(usec) -1 do begin
    gap = usec[i] - usec[i-1]
    if gap gt 5d6 then print, get_routine_name() + strtrim(string(gap * 1d-6), 2) + $
      ' second gap begining at ' + usec2vms( usec[i-1])
    ; If the gap is more than thirty minutes (1.8e9 usec) don't try to interpolate
    ; This is an arbitrary heuristically chosen number that may later be changed.
    if gap gt 1.8e9 then begin
      return
    endif
    ; If there is a gap, add the appropriate times to the array used for interpolation
    while (usec[i] - updated_usec[j]) gt 5d6 do begin
      updated_usec =  [updated_usec, updated_usec[j] + 5000000LL ]
      j = j + 1
    endwhile
    updated_usec =  [updated_usec, usec[i]]
    j = j + 1
  endfor

  ; If no interpolation is necessary, return the inputs unchanged
  if n_elements(usec) eq n_elements(updated_usec) then return

  print, get_routine_name() + 'interpolating from ' + strtrim(string(n_elements(usec)), 2) + ' data points to ' + $
    strtrim(string(n_elements(updated_usec)), 2) + ' to fill quaternion gaps'
    
  updated_value = dblarr(n_elements(updated_usec))
  updated_value[0] = value[0]
  j = 1
  for i = 1, n_elements(updated_usec) - 1 do begin
    if updated_usec[i] eq usec[j] then begin
      updated_value[i] = value[j]
      j++
    endif else begin
      if (j+2) lt n_elements(usec) then begin
        stop_index = j+2
        start_index = j-1
      endif else begin
        stop_index = n_elements(usec) - 1
        start_index = stop_index -3
      endelse
      updated_value[i] = interpol(value[start_index:stop_index], usec[start_index:stop_index], updated_usec[i], /spline)
    endelse
  endfor

;  if keyword_set(plot_data) then begin
;    mid_point = n_elements(usec) / 2
;    orbit_info = get_orbit_info(usec[mid_point], usec[mid_point])
;    p = plot(usec2ad(updated_usec), updated_value,SYMBOL='*', LINESTYLE=6, COLOR='red', $
;      ytitle='housekeeping value, black database, red interpolated', xtitle='AIM day', $
;      title = 'aqinr2bdyest' + strtrim(string(index), 2) + ' for orbit: ' + $
;      strtrim(string(orbit_info.orbit_number), 2), /current)
;    p = plot(usec2ad(usec), value, SYMBOL='*', LINESTYLE=6, COLOR='black', /overplot)
;    p.save, '/home/wbarrett/Downloads/aqinr2bdyest' + strtrim(string(index), 2) + '_orbit_' + $
;      strtrim(string(orbit_info.orbit_number), 2) + '.png'
;    p.close
;  endif
  
  ; Return the updated arrays with the interpolated data
  value = updated_value

end