program finalfocus
 use bmad
 use plot_twiss_data_mod
 implicit none
   type (lat_struct) lat
   type (ele_struct) ele_inf, ele_start, ele_at_s
   type(coord_struct) start_orb, end_orb, orb_at_s
   type(coord_struct), allocatable :: co(:)
   integer nargs, ix,i, ix_ele
   integer track_state
   integer lun
   integer ios
   integer  j
   character*60 lat_file, line
   logical err_flag
   logical create_taylor, slice_elements, create_inverse_taylor
   real(rp) max_beta, min_max_beta,dist
   real(rp) s_start/0./,s_end/0.0/
   real(rp) energy

   namelist /finalfocus_input/start_orb, energy, create_taylor, slice_elements, create_inverse_taylor

!  bmad_com%rel_tol_tracking           = 1.e-8
!  bmad_com%abs_tol_tracking           = 1.e-10
!  bmad_com%rel_tol_adaptive_tracking  = 1.e-10
!  bmad_com%abs_tol_adaptive_tracking  = 1.e-12
!  bmad_com%min_ds_adaptive_tracking   = 1.e-14
!  bmad_com%fatal_ds_adaptive_tracking = 0.
     bmad_com%auto_bookkeeper=.false.
      nargs = command_argument_count()
      if(nargs == 1)then
         call get_command_argument(1,lat_file)
         print *, 'Using ', trim(lat_file)
       else
      lat_file = 'bmad.'
      print '(a,$)',' Lattice file name ? (default= bmad.) '
      read(5,'(a)') line
       call string_trim(line, line, ix)
       lat_file = line
       if(ix == 0) lat_file = 'bmad.'
       print *, ' lat_file = ', lat_file
      endif

       bmad_com%min_ds_adaptive_tracking = 0.00001
       call bmad_parser (lat_file, lat)

     start_orb%vec(1:6)=0
     lun = lunget()
     open(unit=lun, file='finalfocus_input.dat', STATUS ='old')
     read(lun, nml=finalfocus_input, IOSTAT=ios)
     close(unit=lun)

       call reallocate_coord (co, lat%n_ele_max)

       print '(a,6es12.4)',' Just before track_all: start_orb',start_orb%vec

!       lat%ele(0)%value(E_TOT$) = energy * 1.e9
       lat%ele(1:lat%n_ele_track)%alias= 'Record'
!       call set_flags_for_changed_attribute(lat%ele(0), lat%ele(0)%value(E_TOT$))
!       call lattice_bookkeeper(lat, err_flag)
       print *,' Energy [GeV] = ', lat%ele(0)%value(E_TOT$)/1.e9


       lat%ele(1:lat%n_ele_track)%alias= 'trajectory'
       print *,' Length = ', lat%ele(lat%n_ele_track)%s
       co(0)%vec = 0.

       co(0)%vec = co(0)%vec + start_orb%vec
       do i = 1,lat%n_ele_track

        call track1(co(i-1),lat%ele(i), lat%param, co(i))
        if(i < lat%n_ele_track)then
            if(lat%ele(i+1)%name == 'BACKLEG_START')then
              co(i)%vec = co(i)%vec + start_orb%vec
            endif
        endif
          print '(i10,1x,a,1x,7es12.4)',i,lat%ele(i)%name,lat%ele(i)%s, co(i)%vec
       end do
       call lat_make_mat6 (lat, -1, co, err_flag=err_flag)
       call twiss_propagate_all(lat)
     
        do i=1,lat%n_ele_track
!         call type_ele(lat%ele(i), type_zero_attrib=.false.,type_mat6=6, type_taylor=.true., type_control=short$, type_wake=.false., type_floor_coords= .false., nunit=0)
         if(create_taylor)call create_taylor_ele(lat%ele(i),co(i))          
         if(create_inverse_taylor)call create_inv_taylor_ele(lat%ele(i))
         call type_ele(lat%ele(i), type_zero_attrib=.false.,type_mat6=6, type_taylor=.true.,twiss_out=cycles$, type_control=short$, type_wake=.false., type_floor_coords= .false., nunit=0) 
        end do

        if(slice_elements)then
         do while(s_end < lat%ele(lat%n_ele_track)%s)
          call twiss_and_track_at_s(lat, s_end, ele_at_s, co, orb_at_s)          
          write(21,'(es12.4,1x,6es12.4)')s_end, orb_at_s%vec !end_orb%vec
          write(22,'(a16,7es12.4)')ele_at_s%name, ele_at_s%s, ele_at_s%a%beta, &
                   ele_at_s%b%beta, ele_at_s%a%alpha, ele_at_s%b%alpha, &
                   ele_at_s%x%eta,  ele_at_s%x%etap
          s_end = s_end + 0.1

        end do
       else
        do i=1,lat%n_ele_track
          write(22,'(a16,8es12.4)')lat%ele(i)%name, lat%ele(i)%s, lat%ele(i)%a%beta, &
                   lat%ele(i)%b%beta, lat%ele(i)%a%alpha, lat%ele(i)%b%alpha, &
                   lat%ele(i)%x%eta,  lat%ele(i)%x%etap, value_of_attribute(lat%ele(i), 'X_ANGLE', err_flag, .false., 0.0_rp)
       end do
   endif  

         close(unit=21)
         close(unit=22)
         print '(a)', ' fort.21 and fort.22 are written'
         print '(a)',' twiss at end '
         print '(4(a,es12.4))',' beta_x = ',ele_at_s%a%beta,' alpha_x = ',ele_at_s%a%alpha,' beta_y = ',ele_at_s%b%beta,' alpha_y = ',ele_at_s%b%alpha
         call plot_twiss_data(lat)
       end