! map  x,y,z coordinates of point in tokamak to box coordinates and box indices.
!(b1,b2,b3) are unit vectors in box coordinate system and(e1,e2,e3) are unit vectors in tokamak cartesian.
!system are 
!
! b1   | -cos(theta)*cos(phiz)        cos(theta)*sin(phiz)        sin(theta)      |    e1           
! b2  =| -sz*st*sin(phiz)            -sz*st*cos(phiz)             0               |    e2 
! b3   |  sz*st*sin(theta)*cos(phiz) -sz*st*sin(theta)*sin(phiz)  sz*st*cos(theta)|    e3
!
!where theta is sightline elevation = asin((xybapa(ib)-xybsca(ib))/xlbapa(ib)
!phiz = phil - zeta
!phil is angle from source to axis assuming positive ip,ipccw*atan(rtcena(ib)/b)  
!zeta is  zeta of source
!sz is sign of phil
!st is sign of cos(theta). this sign appeared after b2 to be normalized to | cos(theta) |.
!
!b2 is ~ [(0,0,1) x b1] (this definition comes from nbline)
!
  subroutine car2box(ix,r,rb)
    use nbi_com, only: xybapa, xybsca, xlbapa, xlbtna, rtcena, xbzeta, degrad
    use nbi_com, only: nbsbox,nlco, ntrace, nsjdotb, nsnccw,fp
    use tube_mod, only: zzcross,zzdot
    implicit none
    integer, intent(in)  :: ix        ! box index
    real(fp),  intent(in)  :: r(3)      ! coordinates of point in tokamak, [cm]
    real(fp),  intent(out) :: rb(3)      ! coordinates of point in box 'ix', [cm]
    integer :: ib,ipccw
    real(fp)  :: thetal, b, phil
    real(fp)  :: zz, b0(3), that(3), xhat(3), yhat(3)
    real(fp)  :: d(3)
    real(fp)  :: l, x, y

    integer :: jx,jy,jz
    logical :: inside
    real(fp)  :: sign_a,sign_t,xhat1(3),yhat1(3)

    rb=0.0_fp
    ib    = nbsbox(ix)        ! beam index for box
    if(ntrace(ib).ne.0) ib = abs(ntrace(ib))

    ipccw = nsnccw*nsjdotb  

    thetal = asin((xybapa(ib)-xybsca(ib))/xlbapa(ib))  ! sightline elevation
    b      = xlbtna(ib)*cos(thetal)         ! equatorial distance from source to tangent, [cm]
    phil   = ipccw*atan(rtcena(ib)/b)       ! angle from source to axis assuming positive ip
    
    if (.not.nlco(ib)) phil=-phil           ! now relative to positive zeta

    zz = xbzeta(ib)*degrad                  ! zeta of source
    b=sqrt(b*b+rtcena(ib)**2)
    b0(1) = b*cos(zz)                       ! x,y,z of source, [cm]
    b0(2) = b*sin(zz)
    b0(3) = xybsca(ib)

    sign_t=sign(1.0_fp,cos(thetal))
    sign_a=sign(1.0_fp,phil)*sign_t
    that(1) = -cos(thetal)*cos(phil-zz)     ! sightline unit vector
    that(2) =  cos(thetal)*sin(phil-zz)
    that(3) =  sin(thetal)
    xhat1(1)= -sign_a*sin(phil-zz)
    xhat1(2)= -sign_a*cos(phil-zz)
    xhat1(3)= 0.0_fp
    yhat1(1) = sign_t*cos(phil-zz)*sin(thetal)
    yhat1(2) = -sign_t*sin(phil-zz)*sin(thetal)
    yhat1(3) = sign_t*cos(thetal)
    call zzcross((/0.0_fp,0.0_fp,1._fp/),that,xhat)  ! as defined in nbline as rhat direction
    if (phil<0.0_fp) xhat=-xhat                     ! point toward the axis
    xhat = xhat/sqrt(zzdot(xhat,xhat))          ! make unit vector, before this |xhat|^2 = cos^2(thetal)
    call zzcross(that,xhat,yhat)                ! up like direction
    if (yhat(3)<0.0_fp) yhat=-yhat

    d = r-b0            ! vector relative to source
! along sightline
    rb(1) = zzdot(d,xhat)   ! x, [cm]
    rb(2) = zzdot(d,yhat )  ! y, [cm]
    rb(3) = zzdot(d,that)   ! l, [cm]

!mg    call boxfind(rb(1),rb(2),rb(3),jx,jy,jz,inside)
!mg    if (inside) then
!mg       idx_box(1)=jx
!mg       idx_box(2)=jy
!mg       idx_box(3)=jz
!mg    else
!mg       idx_box=0
!mg    end if
  end subroutine car2box
!
! map  x,y,z coordinates of point in box coordinates to tokamak 
!(b1,b2,b3) unit vectors in box coordinate system and(e1,e2,e3) unit vectors in tokamak cartesian
!system are 
!
! e1   | -cos(theta)*cos(phiz)   -sz*st*sin(phiz)   sz*st*sin(theta)*cos(phiz) |    b1           
! e2  =|  cos(theta)*sin(phiz)   -sz*st*cos(phiz)  -sz*st*sin(theta)*sin(phiz) |    b2 
! e3   |  sin(theta)              0                 sz*st*cos(theta)           |    b3
!
!where theta is sightline elevation = asin((xybapa(ib)-xybsca(ib))/xlbapa(ib)
!phiz = phil - zeta
!phil is angle from source to axis assuming positive ip,ipccw*atan(rtcena(ib)/b)  
!zeta is  toroidal angle of start point of the source
!sz is sign of phil
!st is sign of cos(theta)
!
!
  subroutine box2car(ix,rb,r)
    use nbi_com, only: xybapa, xybsca, xlbapa, xlbtna, rtcena, xbzeta, degrad
    use nbi_com, only: nbsbox,nlco, ntrace, nsjdotb, nsnccw,fp
    use tube_mod, only: zzdot
    implicit none
    integer, intent(in)  :: ix        ! box index
    real(fp),  intent(in)  :: rb(3)      ! coordinates of point in box 'ix', [cm]
    real(fp),  intent(out) :: r(3)      ! coordinates of point in tokamak, [cm]

    integer :: ib,ipccw
    real(fp)  :: thetal, b, phil
    real(fp)  :: zz, b0(3), that(3), xhat(3), yhat(3)
    real(fp)  :: d(3)
    real(fp)  :: l, x, y
    real(fp)  :: sign_a,sign_t,sign_p

    integer :: jx,jy,jz
!mg    logical :: inside
    r=0.0_fp
    ib    = nbsbox(ix)        ! beam index for box
    if(ntrace(ib).ne.0) ib = abs(ntrace(ib))

    ipccw = nsnccw*nsjdotb  

    thetal = asin((xybapa(ib)-xybsca(ib))/xlbapa(ib))  ! sightline elevation
    b      = xlbtna(ib)*cos(thetal)         ! equatorial distance from source to tangent, [cm]
    phil   = ipccw*atan(rtcena(ib)/b)       ! angle from source to axis assuming positive ip
    
    if (.not.nlco(ib)) phil=-phil           ! now relative to positive zeta

    zz = xbzeta(ib)*degrad                  ! zeta of source
    call car2box(ix,r,b0)
    sign_t=sign(1.0_fp,cos(thetal))
    sign_p=sign(1.0_fp,phil)
    sign_a=sign_p*sign_t
    xhat(1) =  -sign_t*sin(phil-zz)*sign_p
    xhat(2) =  sign_a*sin(thetal)*cos(phil-zz)*sign_p 
    xhat(3) = -sign_a*cos(thetal)*cos(phil-zz)*sign_p     ! sightline unit vector
    yhat(1) =  -sign_t*cos(phil-zz)*sign_p
    yhat(2) =  -sign_a*sin(thetal)*sin(phil-zz)*sign_p
    yhat(3) =  sign_p*cos(thetal)*sin(phil-zz)*sign_p 
    that(1) =  0.0_fp
    that(2) =  sign_a*cos(thetal)*sign_p
    that(3) =  sign_p*sin(thetal)*sign_p

    d =  rb - b0           ! vector relative to tokamak
    r(1) = zzdot(d,xhat)   ! x, [cm]
    r(2) = zzdot(d,yhat)   ! y, [cm]
    r(3) = zzdot(d,that)   ! z, [cm]

!for debug
!    call car2box(ix,r,b0) !b0 must be  = rb

  end subroutine box2car
