!........................................................................
!
! Subroutine : SOLENOID_COMPENSATION(lat,ip_decouple)
!
! Description: Compute values of sc_sk_q01, sc_sk_q02 and sk_q02 to 
!              compensate solenoid
!                if ip_decouple = 1 then c11,c12,c22 at ip are zero
!                if ip_decouple = 0 then just global coupling with two 
!                                        parameters
!
! Arguments  : 
!    Input:
!        LAT  -- lat_struct: Lat
!        IP_DECOUPLE : integer
!
!    Output:
!        LAT -- elements updated to reflect comp         
!
! Mod/Commons: use bmad   
! 
! Calls      :      
!
! Author     : 
!
! Modified   :     
!             
!  print '(a18,2e12.4)','      mat(3,1:2) =', m(0)%mat(3,1:2)
!  print '(a18,2e12.4)','      mat(4,1:2) =', m(0)%mat(4,1:2)
! compute derivatives
!     vec(1) = -mat(4,1)*mat(1,2)+mat(4,2)*mat(1,1)
!     vec(2) =  mat(3,1)*mat(2,2)-mat(3,2)*mat(1,2)
!........................................................................

subroutine solenoid_compensation (lat, ip_decouple, ele_names)

  use bmad

  implicit none
  
  type (lat_struct)  lat

  type mat_struct
     real(rp) mat(6,6)
     real(rp) vec(4)
     real(rp) delta_vec(4)
  end type mat_struct

  type (mat_struct) m(0:4)
  type (all_pointer_struct) attribute(4)
  type (ele_pointer_struct) e(4)
  type (ele_struct), pointer :: ele1, ele2

  real(rp) detmn_dag, val_0(4),del(4), x(4), matrix(4,4)
  real(rp) epsi(4)
  real(rp) l_min, circum
  
  integer n, i, n_param, ip_decouple
  integer ixe, ix, k, j
  logical :: first_time = .true.
  logical :: debug = .false.
  character*40 ele_names(4)
  character*12 start_name
  character*12 :: end_name = 'IP_L0_END'
!  character*12 start_name/'SK_Q03E'/,end_name/'IP_L0_END'/
  save n_param, attribute, epsi
  save del, circum, l_min, start_name, ixe

  data epsi /4*0.0001/


  if (first_time) then
     n_param = 0
     do i=1,size(ele_names)
        call string_trim(ele_names(i), ele_names(i),ix)
        if (ix >0) n_param=n_param+1
     end do
     
     if (n_param < 2 .and. n_param  > 4) then
        print *,' SOLENOID_COMPENSATION: n_param /= 2,3, or 4'
        call err_exit
     endif
        
     ip_decouple = n_param - 2
        
     do n=1,n_param
        call element_locator( ele_names(n), lat, ix)
        if (ix == -1) then
           print *,' can t find element ', ele_names(n)
           call err_exit
        endif
        e(n)%ele => lat%ele(ix)
        if (e(n)%ele%key == overlay$) then
           attribute(n)%r => e(n)%ele%control%var(1)%value
        elseif (ele_names(n)(1:3) == 'Q00') then
          attribute(n)%r => e(n)%ele%value(tilt$)
        else
           attribute(n)%r => e(n)%ele%value(k1$)
        endif           

     end do

    !

     del(1:4)=0.0001

     circum = lat%param%total_length
     l_min = circum
     do i = 1, n_param
        if (e(i)%ele%n_slave /= 0) then
           ele1 => pointer_to_slave(e(i)%ele, 1)
           ele2 => pointer_to_slave(e(i)%ele, e(i)%ele%n_lord)
           
           do j = ele1%ix_ele, ele2%ix_ele
              if(lat%ele(j)%s < circum/2)cycle
              if (lat%ele(j)%s > l_min) cycle
              l_min = lat%ele(j)%s
              start_name = lat%ele(j)%name
           end do
        endif
     end do
    
     if (n_param >=3) then
        ixe = index(end_name, ' ') -1
        if(lat%ele(lat%n_ele_track)%name(1:ixe) /= end_name(1:ixe))then
           print *,' SOLENOID COMPENSATION: last element is not ',end_name
           print *,' Automatic compensation will not work '
           print *,' Append ',end_name, ' to layout file'
           call err_exit
        endif

     endif
  


!   if(ip_decouple == 1) then
!     n_param = 3
!   elseif(ip_decouple == 0)then

     if(n_param == 2) end_name = 'SK_Q03W'

!     n_param = 2
!   elseif(ip_decouple == 2)then
!     n_param = 4
!     start_name = 'SK_Q04E'
!  endif

     first_time = .false.
  endif

  call transfer_line_matrix_calc (lat, start_name, end_name, m(0)%mat, detmn_dag)
  call mat_to_vec (m(0)%mat, m(0)%vec, n_param)


  do while(any(abs(m(0)%vec(1:n_param)) > epsi(1:n_param)))
 
     do i=1,n_param
        val_0(i) = attribute(i)%r
     end do

     call transfer_line_matrix_calc (lat, start_name, end_name, m(0)%mat, detmn_dag)
     call mat_to_vec (m(0)%mat, m(0)%vec, n_param)

     if(debug)then
        print *,' solenoid_compensation: '
        print '(a12,3e12.4)','       ks = ', val_0
        print '(a12,3e12.4)','      vec = ', m(0)%vec
     endif
     !  print '(a18,2e12.4)','      mat(3,1:2) =', m(0)%mat(3,1:2)
     !  print '(a18,2e12.4)','      mat(4,1:2) =', m(0)%mat(4,1:2)

     ! compute derivatives

     do i=1,n_param
        attribute(i)%r = val_0(i) + del(i)
        call lat_make_mat6 (lat, e(i)%ele%ix_ele)
        call transfer_line_matrix_calc (lat, start_name, end_name, m(i)%mat, detmn_dag)
        call mat_to_vec( m(i)%mat, m(i)%vec, n_param)
        m(i)%delta_vec = (m(i)%vec - m(0)%vec)/del(i)
        attribute(i)%r = val_0(i)
        call lat_make_mat6(lat, e(i)%ele%ix_ele)
     end do

     forall(i=1:n_param) matrix(i,1:n_param) = m(i)%delta_vec(1:n_param)
        
     call gjdet_sc( matrix(1:n_param,1:n_param), -m(0)%vec(1:n_param), x(1:n_param), n_param)

     do i=1,n_param
        attribute(i)%r = val_0(i) + x(i)
        call lat_make_mat6(lat, e(i)%ele%ix_ele)
     end do

  end do

  return

end subroutine solenoid_compensation


subroutine mat_to_vec( mat, vec, n_param)
  use bmad

  implicit none

  integer n_param
  real(rp) mat(6,6), vec(4)

  if(n_param == 3)then
     vec(1) = mat(3,1)
     vec(2) = mat(3,2)
     vec(3) = -mat(4,1)*mat(1,2)+mat(4,2)*mat(1,1)
  endif
  if(n_param == 2)then
     !     vec(1) = -mat(4,1)*mat(1,2)+mat(4,2)*mat(1,1)
     !     vec(2) =  mat(3,1)*mat(2,2)-mat(3,2)*mat(1,2)
     vec(1) = mat(3,2)
     vec(2) = mat(4,1)
  endif
  if(n_param == 4)then
     vec(1) = mat(1,3)
     vec(2) = mat(1,4)
     vec(3) = mat(2,3)
     vec(4) = mat(2,4)
  endif
  
  return

end subroutine mat_to_vec