;+
;Procedure: tvector_rotate
;
;Purpose:  rotates a set of vectors by a set of coordinate
;          transformation matrices inputs and outputs are tplot
;          variables.  This is designed mainly for use with
;          fac_matrix_make.pro and minvar_matrix_make, but can be used
;          for more general purposes
;
;Warning:
; The transformation matrices generated by
;fac_matrix_make,thm_fac_matrix_make, and minvar_matrix_make are
;defined relative to a specific coordinate system.  This means that if
;you use vectors in one coordinate system to generate the rotation
;matrices they will only correctly transform data from that coordinate
;system into the functional coordinate system.  For example if you use
;magnetic field data in gsm to generate Rgeo transformation matrices
;using fac_matrix_make then the vectors being provided to tvector
;rotate to be transformed by those matrices should only be in gsm coordinates.

;
;Arguments:
;
; mat_var_in: the name of the tplot variable storing input matrices
; The y component of the tplot variable's data struct should be
; an Mx3x3 array, storing a list of transformation matrices. 
; 
;
; vec_var_in: the name of the tplot variable storing input
;vectors. You can use globbing to rotate several tplot variables
;storing vectors with a single matrix. The y component of the tplot variable's
; data struct should be an Nx3 array.  
;
;
;
; newname(optional): the name of the output variable, defaults to 
;                    vec_var_in + '_rot'
;                    If you use type globbing in the vector variable
;                    This option will be disabled
;
; error(optional): named variable in which to return the error state
; of the computation.  1 = success 0 = failure
; 
; 
; invert(optional):  If matrix_var naturally transforms vector_var from Coord A to B,
;                    (ie Vb = M#Va where # denotes matrix multiplication) then setting
;                    this keyword will transform vector_var from Coord B to A( ie Va = M^T#Vb 
;                    This is done by transposing the input matrices, as it is a property of 
;                    rotation matrices that M#M^T = I and M^T#M = I(ie M^T = M^-1) 
;
;
;CALLING SEQUENCE:
;
; tvector_rotate,'matrix_var','vector_var',newname = 'out_var',error=error
;
; tvector_rotate,'matrix_var','vector_var'
;
;Notes: 
; 1.  mat_var_in should store rotation matrices.  (ie the columns of any
;     matrix in mat_var_in should form an orthonormal basis)
;     tvector_rotate will test and warn if input matrices are invalid.
;   
; 2.  transformation matrices generally only transform from one particular basis
;     to another particular basis.  Since tvector_rotate as no way to test that
;     vector_var is in the correct basis, you need to be very careful that
;     vector_var has the correct coordinate system and representation
;     so that it can correctly transform the data.
;     
;
;3.   If M!=N, then M must be >= 2 (where M,N refer to the dimensions of the
;     input variables.)
;
;4.  Also If the x components of mat_var_in and vec_var_in data structs
;    do not match then, the matrices will be interpolated to match the
;    cadence of the vector data.  Interpolation is done by turning the
;    matrices into quaternions and performing spherical linear
;    interpolation.  As noted above this interpolation behavior will
;    not be predictable if the matrices do anything other than rotate.
;
;5.  The user should note that if the timestamps of mat_var_in are not
;    monotonic(ascending or identical) or if the timestamps vec_var_in are not strictly
;    monotonic(always ascending) the program will not work correctly.  It is the user's 
;    responsibility to remove duplicate or non sequential timestamps from the data 
;
;
; SEE ALSO:  mva_matrix_make.pro, fac_matrix_make.pro,rxy_matrix_make
;
; $LastChangedBy: pcruce $
; $LastChangedDate: 2009-06-24 12:31:07 -0700 (Wed, 24 Jun 2009) $
; $LastChangedRevision: 6317 $
; $URL: svn+ssh://thmsvn@ambrosia.ssl.berkeley.edu/repos/ssl_general/trunk/cotrans/special/tvector_rotate.pro $
;-

;helper function
;used to determine if values are equal within some standard of computational error
function ctv_err_test,vals,val

  err = 1d-5

  return,vals ge val-err and vals le val+err

end

;helper function
;returns the determinant of a list of 3x3 matrices

function ctv_determ_mats,m

  compile_opt hidden

  return,reform(m[*,0,0] * m[*,1,1] * m[*,2,2] $
               -m[*,0,0] * m[*,1,2] * m[*,2,1] $
               -m[*,0,1] * m[*,1,0] * m[*,2,2] $
               +m[*,0,1] * m[*,1,2] * m[*,2,0] $
               +m[*,0,2] * m[*,1,0] * m[*,2,1] $
               -m[*,0,2] * m[*,1,1] * m[*,2,0])
  
end

;helper function
;determines if a list of 3x3 matrices are identity matrices
;will return the indexes of the identity matrices in the list of matrices
function ctv_identity_mats,m
 
  compile_opt hidden
  
  dim = dimen(m)
  
  out = lonarr(dim[0])
  
  return,where(ctv_err_test(m[*,0,0],1) and ctv_err_test(m[*,0,1],0) and ctv_err_test(m[*,0,2],0) and $
               ctv_err_test(m[*,1,0],0) and ctv_err_test(m[*,1,1],1) and ctv_err_test(m[*,1,2],0) and $
               ctv_err_test(m[*,2,0],0) and ctv_err_test(m[*,2,1],0) and ctv_err_test(m[*,2,2],1))

end

;helper function
;vectorized multiplication of two lists of 3x3 matrices 
function ctv_mm_mult,m1,m2

  compile_opt hidden
  
  out = make_array(dimen(m1),type=size(m1,/type))
  
  out[*,0,0] = total(m1[*,0,*] * m2[*,*,0],3)
  out[*,1,0] = total(m1[*,1,*] * m2[*,*,0],3)
  out[*,2,0] = total(m1[*,2,*] * m2[*,*,0],3)
  
  out[*,0,1] = total(m1[*,0,*] * m2[*,*,1],3)
  out[*,1,1] = total(m1[*,1,*] * m2[*,*,1],3)
  out[*,2,1] = total(m1[*,2,*] * m2[*,*,1],3)
  
  out[*,0,2] = total(m1[*,0,*] * m2[*,*,2],3)
  out[*,1,2] = total(m1[*,1,*] * m2[*,*,2],3)
  out[*,2,2] = total(m1[*,2,*] * m2[*,*,2],3)
  
  return,out

end

;helper function
;verifies whether a list of matrices 
;contains valid rotation matrices.
;This is determined using 2 constraints.
;#1 Where determ(matrix) eq 1
;#2 Where matrix#transpose(matrix) eq I
;returns 0 if there are no valid mats,
;returns 1 if there are some valid mats
;returns 2 if there are all valid mats
function ctv_verify_mats,m

  compile_opt hidden
  
  idx1 = where(ctv_err_test(ctv_determ_mats(m),1))
  idx2 = ctv_identity_mats(ctv_mm_mult(m,transpose(m,[0,2,1])))
  
  idx3 = ssl_set_intersection(idx1,idx2)
  
  dim = dimen(m)
  
  if idx3[0] eq -1 then begin
    return,0
  endif else if n_elements(idx3) eq dim[0] then begin
    return,2
  endif else begin
    return,1
  endelse

end

;helper function
;calculates the norm of a bunch of vectors simultaneously
function ctv_norm_vec_rot,v

COMPILE_OPT HIDDEN

if not keyword_set(v) then return, -1L

if size(v,/n_dim) ne 2 then return,-1L

return, sqrt(total(v^2,2))

end

;helper function
;normalizes a bunch of vectors simultaneously
function ctv_normalize_vec_rot,v

COMPILE_OPT HIDDEN

if not keyword_set(v) then return, -1L

if size(v,/n_dim) ne 2 then return,-1L

n_a = ctv_norm_vec_rot(v)

if(size(n_a,/n_dim) eq 0 && n_a eq -1L) then return,-1L

v_s = size(v,/dimension)

;calculation is pretty straight forward
;we turn n_a into an N x D so computation can be done element by element
n_b = rebin(n_a,v_s[0],v_s[1])

return,v/n_b

end

;helper function
;vectorized fx to multiply n matrices by n vectors
function ctv_mx_vec_rot,m,x

COMPILE_OPT HIDDEN

;input checks
if(not keyword_set(m)) then return, -1L

if(not keyword_set(x)) then return, -1L

m_s = size(m,/dimension)

x_s = size(x,/dimension)

;make sure number of dimensions in input arrays is correct
if(n_elements(m_s) ne 3) then return, -1L

if(n_elements(x_s) ne 2) then return, -1L

;make sure dimensions match
if(not array_equal(x_s,[m_s[0],m_s[1]])) then return,-1L

if(not array_equal(m_s,[x_s[0],x_s[1],x_s[1]])) then return,-1L

;calculation is pretty straight forward
;we turn x into an N x 3 x 3 so computation can be done element by element
y_t = rebin(x,x_s[0],x_s[1],x_s[1])

;custom multiplication requires rebin to stack vector across rows,
;not columns
y_t = transpose(y_t, [0, 2, 1])

;9 multiplications and 3 additions per matrix
y = total(y_t*m,3)

return, y

end

pro tvector_rotate,mat_var_in,vec_var_in,newname = newname,error=error,invert=invert

if keyword_set(error) then error = 0

if not keyword_set(mat_var_in) then begin
    dprint,'FX requires the matrix variable to be set'
    return
endif

if tnames(mat_var_in) eq '' then begin
    dprint,'FX requires the matrix variable to be set'
    return
endif

if not keyword_set(vec_var_in) then begin
    dprint,'FX requires vector variable to be set'
    return
endif

tn = tnames(vec_var_in)

if size(tn,/n_dim) eq 0 && tn eq '' then begin
    dprint,'FX requires vector variable to be set'
    return
endif

if not keyword_set(newname) then $
  newname = [vec_var_in + '_rot'] $
else $
  newname = [newname]


if n_elements(tn) gt 1 then newname = tn + '_rot'


;loop over possibly many vectors
for i = 0,n_elements(tn) -1L do begin

    ;tinterpol_mxn,mat_var_in,tn[i],newname = mat_var_in+'_interp_temp',error=error_state

   ;load the matrices and validate dimensions
   get_data,mat_var_in,data=m_d,dlimits=m_dl ;rotation matrices and input abcissa values
   
   m_d_s = size(m_d.y,/dimensions)

   if(n_elements(m_d_s) ne 3 || m_d_s[1] ne 3 || m_d_s[2] ne 3) then begin

      dprint,'FX requires matrix data to contain an Nx3x3 array'
      return
   endif

   ;load the vectors and validate dimensions
   get_data,tn[i],data=v_d,dlimits=v_dl ;output abcissa values
   
   v_d_s = size(v_d.y,/dimensions)

   if(n_elements(v_d_s) ne 2 || v_d_s[1] ne 3) then begin
      dprint,'FX requires vector data to contain an Nx3 Array'
      return
   endif

   idx = where(m_d.x ne v_d.x)

   if n_elements(m_d.x) eq n_elements(v_d.x) && idx[0] eq -1 then begin

      m_d_y = m_d.y
      
      verify = ctv_verify_mats(m_d_y)

   endif else begin
      
      if n_elements(m_d.x) gt 1 then begin
      
        idx = where((m_d.x[1:n_elements(m_d.x)-1]-m_d.x[0:n_elements(m_d.x)-2]) lt 0)

        if(idx[0] ne -1) then begin

           dprint,'matrix timestamps not monotonic please remove any non-ascending timestamps from your data'
           return

        endif
      endif
      
      if n_elements(v_d.x) gt 1 then begin
    
        idx = where((v_d.x[1:n_elements(v_d.x)-1]-v_d.x[0:n_elements(v_d.x)-2]) le 0)

        if(idx[0] ne -1) then begin

           dprint, 'vector timestamps not monotonic please remove any non-ascending or repeated timestamps from your data'
           return

        endif
        
      endif
      
      if ~array_equal(m_d.x,v_d.x) then begin
        dprint,'WARNING: time arrays do not match.  Matrices will be interpolated to match the times of input vector array'
      endif
   
      verify = ctv_verify_mats(m_d.y)
   
      ;using quaternion library transform into quanternions
      q_in = mtoq(m_d.y)

      ;interpolate quaternions
      q_out = qslerp(q_in,m_d.x,v_d.x)

      ;turn quaternions back into matrices
      m_d_y = qtom(q_out)

   endelse
   
   if verify eq 0 then begin
     dprint,'ERROR: Input matrices are not valid rotation matrices or error is greater than +-1e-5. Results are probably incorrect.'
     
     ;decided against forced failure, as extreme numerical instability can create false positives for invalid tests
     ;return
   endif else if verify eq 1 then begin
     dprint,'WARNING: Some input matrices are not valid rotation matrices. Operation will continue, but you may want to verify the result'
     
   endif
     
   if(size(m_d_y,/n_dim) eq 0 && m_d_y[0] eq -1L) then begin

      dprint,'failed to interpolate rotation matrix array'

      return

   endif
   
   ;rotate vectors using matrix right multiplication
   
   ;if invert is set, then we transpose rotation matrices to construct inverse rotation matrix
   if keyword_set(invert) then begin
     v_t = ctv_mx_vec_rot(transpose(m_d_y,[0,2,1]),v_d.y)
   endif else begin
     v_t = ctv_mx_vec_rot(m_d_y,v_d.y)
   endelse
   
   if(size(v_t,/n_dim) eq 0 && v_t eq -1L) then begin
      dprint,'FX failed to perform mx operation'
      return
   endif

   ;update meta data
   ;and store output

    ;if the vector dlimits struct is unavailable create one
    if(size(v_dl,/n_dim) eq 0 && v_dl eq 0) then begin
        dprint,'dlimits for input vector should be set'
        dprint,'Creating default dlimits structure'

        v_data_att = {coord_sys:'undefined',units:''}
        v_dl = {data_att:v_data_att,labflag:0,labels:make_array(v_d_s[1],/string,value='')}

    endif

    ;if the matrix dlimits struct is unavailable create one
    if(size(m_dl,/n_dim) eq 0 && m_dl eq 0) then begin
        dprint,'m_dl coord_sys unset, unable to make coordinate system determination'
        
        m_data_att = {coord_sys:'undefined',units:''}
        m_dl = {data_att:m_data_att,labflag:0,labels:make_array(v_d_s[1],/string,value='')}
    endif


    ;make sure it has data_att
    str_element,v_dl,'data_att',SUCCESS=s

    if(s eq 0) then begin

        v_data_att = {coord_sys:'undefined',units:''}
        
        str_element,/add,v_dl,'data_att',v_data_att

    endif

    ;make sure it has ytitle

    str_element,v_dl,'ytitle',SUCCESS=s

    if(s eq 0) then begin
 
        v_ytitle = ''

        str_element,/add,v_dl,'ytitle',v_ytitle

    endif

    ;make sure it has appropriate data_att elements
    str_element,v_dl.data_att,'coord_sys',SUCCESS=s

    if(s eq 0) then begin

        v_data_att = v_dl.data_att 

        str_element,/add,v_data_att,'coord_sys','undefined'

        str_element,/add,v_dl,'data_att',v_data_att

        
    endif

    str_element,v_dl.data_att,'units',SUCCESS=s

    if(s eq 0) then begin

        v_data_att = v_dl.data_att 

        str_element,/add,v_data_att,'units',''

        str_element,/add,v_dl,'data_att',v_data_att

    endif

    v_dl.data_att.coord_sys = m_dl.data_att.coord_sys
    
    if keyword_set(invert) then begin
      v_dl.data_att.units = v_dl.data_att.units+'_irot'
    endif else begin
      v_dl.data_att.units = v_dl.data_att.units+'_rot'
    endelse
    
    v_dl.ytitle = newname[i] + '!C' + v_dl.data_att.units
     

    ;no longer take labels from transformation matrix
    ;if m_dl.labflag eq 1 then v_dl.labels = m_dl.labels

    str_element,v_d,'v',SUCCESS=s

    if(s) then $
      v_d = {x:v_d.x,y:v_t,v:v_d.v} $
    else $
      v_d = {x:v_d.x,y:v_t} 

    store_data,newname[i],data=v_d,dlimits = v_dl

endfor

error = 1

return

end
