!+
! Subroutine special_procedure (u, graph, line_in)
!
! This subroutine is for custom procedures
!-
                                  
subroutine special_procedure (u, graph, line_in)

use cesrv_struct
use cesrv_interface
use nr

implicit none

type (universe_struct), target :: u
type (graph_struct) graph
type (var_struct), pointer :: vp

integer i1, i2, ix, ios

character(*) line_in
character(80) line
character(16) which

!

type (shaking_modes_struct) shake(0:120)

real(rp) w(2), v(2,2), mat24(2,4), w_sort(4), mat42(4,2), check42(4,2), c(2)
real(rp) dr_a, dr_b

integer i, j, k, iu

namelist / shake_data / shake

!
call string_trim (line_in, line, ix)
which = line(:ix)
line = line(ix+1:)

select case (which)

!---------------------------------------------

case ('quad')

  print *, 'Name   k1_model   k1_saved   model-saved/saved'
  do i = lbound(u%quad_k1%v, 1), ubound(u%quad_k1%v, 1)
    vp => u%quad_k1%v(i)
    if (vp%dvar_dcu == 0 .or. .not. vp%exists) cycle
    print '(a8, 3f12.6)', vp%name, vp%model, vp%saved, vp%model/vp%saved - 1
  enddo

!---------------------------------------------
! This calculates u%but_to_mode from shaking data.
! Alternative is to read in u%but_to_mode from an external file. See read_mode_matrix procedure.

case ('wolski')

  ! phase data anal

  iu = lunget()
  open (iu, file = u%phase%file_name)
  read (iu, nml = shake_data, iostat = ios)
  if (ios /= 0) then
    print *, 'Not able to read shake data!'
    return
  endif
  close(iu)

  ! Idea: u%but_to_mode%mat is a 2x4 matrix such that
  !   mat * but_a = (a_mode_x_amp, 0)
  !   mat * but_b = (0, b_mode_y_amp)
  ! where
  !   but_a(4) = sign corrected button values for a-mode shaking. 
  !   but_b(4) = sign corrected button values for b-mode shaking.
  ! It is assumed that the button phases for a-mode or b-mode shaking
  ! are in-phase or out-of-phase with each other

  do i = 0, 120
    if (.not. u%cbar%m12%d(i)%good_dat) cycle
    call wolski_this_det (u%raw_phase_x%det(i), u%raw_orbit%det(i), mat42(:,1), dr_a)
    call wolski_this_det (u%raw_phase_y%det(i), u%raw_orbit%det(i), mat42(:,2), dr_b)    
    check42 = mat42
    call svdcmp (mat42, w, v)
    ! invert using two largest w elements
    v(:,1) = v(:,1) / w(1)
    v(:,2) = v(:,2) / w(2)
    mat24 = matmul(v, transpose(mat42))
    u%but_to_mode(i)%mat(1,:) = mat24(1,:) * dr_a 
    u%but_to_mode(i)%mat(2,:) = mat24(2,:) * dr_b
    c = matmul(u%but_to_mode(i)%mat, check42(:,1)) ! debug check: Should look like (val, 0)
    c = matmul(u%but_to_mode(i)%mat, check42(:,2)) ! debug check: Should look like (0, val)
    !! u%but_to_mode(i)%mat = u%but_to_mode(i)%mat * 4.98  !!! FUDGE !!!
  enddo

  logic%wolski_normal_mode_calc_on = .true.

!---------------------------------------------
! No match

case default
  print *, 'ERROR: CANNOT MATCH SPECIAL PROCEDURE NAME: ', which

end select

!----------------------------------------------------------------------
contains

subroutine wolski_this_det (ac_sig, dc_sig, dbut_major, dr_major)

type (raw_det_struct) :: ac_sig, dc_sig

real(rp) a_in(4), a_out(4), r_major(2), hv0(3), hv_in(3), hv_out(3)
real(rp) dr_in(2), dr_out(2), t_major, dbut_major(4), dr_major, dr(2)

! Model signal as:
!    A_but = Amp * cos(t + phase)
!          = Amp * cos(phase) * cos(t) - Amp * sin(phase) * sin(t)
!          = A_in * cos(t) + A_out * sin(t)
! Factor of 100 comes from bpm system using this to scale the button values.

a_in  =  ac_sig%amp * cos(ac_sig%phase*pi/180) / 100
a_out = -ac_sig%amp * sin(ac_sig%phase*pi/180) / 100

call nonlin_orbit(i, dc_sig%amp, hv0)
call nonlin_orbit(i, dc_sig%amp + a_in, hv_in)
call nonlin_orbit(i, dc_sig%amp + a_out, hv_out)

dr_in  = hv_in(1:2)  - hv0(1:2)
dr_out = hv_out(1:2) - hv0(1:2)

! The AC beam motion is described by:
!       dr_in * cost(t) + dr_out * sin(t)
! Find the intersection of this ellipse with the major axis.

t_major = atan2(sum(dr_in * dr_out), sum(dr_in**2) - sum(dr_out**2)) / 2

dr = dr_in * cos(t_major) + dr_out * sin(t_major)
dr_major = sqrt(dot_product(dr, dr))
dbut_major = (a_in * cos(t_major) + a_out * sin(t_major)) / sum(dc_sig%amp)
if (dr(1) + dr(2) < 0) dbut_major = -dbut_major

end subroutine

end subroutine