c	cal betax at sextupoles S2 S3 based on measured values at Q3,Q4 
C       This program gives Twiss functions at the 36AM SOURCE POINT
c       THE AVERAGE BETA VALUE IS BASED ON VALUES AT THE ENTRANCE,
C       CENTER, AND EXIT OF THE QUADRUPOLE.
c       THE ANALYSIS IS IN TERMS OF A SECTORS. THE VALUES AT THE 
C       SEXTUPOLES ARE GOOD FOR EITHER SECTORS A OR B

C       PRODUCES RMS ERRORS BY MAKING 500  RUNS USING RMS MEASUREMENT 
C       ERRORS
c I changed the definition of flq as being the fuill quad length. L.Emery.
		implicitreal*8(a-h,o-z)
		real*4 ran

		character*4 plane, source
		integer*4 verbose, solutions, notConverged, nonPhysical
		real*8 betaQ4,errorQ4,knlQ4,betaQ3,errorQ3,knlQ3
      common /cmdLineArgs/betaQ4,errorQ4,knlQ4,betaQ3,errorQ3,knlQ3,plane,verbose,source
      integer n, argc
      character*128 argv

      n = iargc()
		if (n.eq.0) then
         call readCmdLineArgs(n, '')
		endif

      do i = 1, n
         call getarg( i, argv)
c     can only send one argument at a time.
			argc = 1
         call readCmdLineArgs(argc, argv)
      enddo
		if (betaQ3.eq.0) then
			write(*,*) 'Invalid beta for Q3.'
			stop
		endif
		if (betaQ4.eq.0) then
			write(*,*) 'Invalid beta for Q4.'
			stop
		endif
		if (knlQ3.lt.0) then
			write(*,*) 'knlQ3 must be >=0.'
			stop
		endif
		if (knlQ4.lt.0) then
			write(*,*) 'knlQ4 must be >=0.'
			stop
		endif
		if (plane(1:1) .eq. '') then
			write(*,*) 'No plane specified.'
			stop
		endif
		
		sq3=6.17
		ss2=7.38
		ss3=11.14
		sq4=12.3
		flq = 0.50				   		! Q3 AND Q4 LENGTH
		ss2=ss2-sq3-flq/2d0  		  ! DISTANCE  FROM EXIT OF Q3 TO CENTER OF S2
		ss3=sq4-ss3-flq/2d0			  ! DISTANCE FROM S3 TO ENTRANCE OF Q4
		sbm=10.79
		if ( source(1:2) .eq. 'AM' ) then
			sbm = sbm - 31.0 * 3.06/ 32. - sq3 - flq/ 2d0 ! DISTANCE FROM SQ3 EXIT TO 36AM SP
		else if (source(1:2) .eq. 'BM' ) then
			sbm = sq4 - sbm - flq/ 2d0 + 3.06/8. ! DISTANCE FROM END OF BM TO ENTRANCE OF Q4
		endif
C     THIS IS ALSO THE DISTANCE FROM EXIT OF Q4
C     TO THE SOURCE POINT IN B MAGNETS
C     THE FOLLOWING ARE NOMINAL K1 VALUES AT Q3 AND Q4.
C     YOU CAN SUPPLY YOUR OWN AT THIS POINT.
C     Aij and Bij are half quad transfer matrices at Q3,Q4
c     write(6,*) 'Input K1 values for Q3,Q4'
		fkq3 = knlQ3 / flq
		fkq4 = knlQ4 / flq
		fkq3=dsqrt(fkq3)
		fkq4=dsqrt(fkq4)
c		transport through half the quad
		x = fkq3 * flq/ 2d0
		if ( x .ne. 0 ) then
			if ( plane(1:1) .eq. 'x' ) then
				a11=dcosh(x)
				a12=dsinh(x)/fkq3
				a21=a12*fkq3**2
			else if ( plane(1:1) .eq. 'y' ) then
				a11=dcos(x)
				a12=dsin(x)/fkq3
				a21=-a12*fkq3**2
			endif
		else
			a11 = 1d0
			a12 = flq/ 2d0
			a21 = 0d0
		endif
c		transport through half the quad
		x=fkq4*flq/ 2d0
		if ( plane(1:1) .eq. 'x' ) then
			if ( x .ne. 0 ) then
				b11=dcosh(x)
				b12=dsinh(x)/fkq4
				b21=b12*fkq4**2
			else
				b11 = 1d0
				b12 = flq/2d0
				b21 = 0d0
			endif
			ss = sq4 - sq3 - flq - 3.06 + 3.05686 ! replace mag l by rho*sin(theta)
		else if ( plane(1:1) .eq. 'y' ) then
			if ( x .ne. 0 ) then
				b11=dcos(x)
				b12=dsin(x)/fkq4
				b21=-b12*fkq4**2
			else
				b11 = 1d0
				b12 = flq/2d0
				b21 = 0d0
			endif
			ss = sq4 - sq3 - flq
		endif
c     write(6,*) ' input betax and error at q3'
c     read(5,*) b0b0,err0
		b0b0 = betaQ3
		err0 = errorQ3
c     write(6,*) ' input betax and error at q4'
c     read(5,*) b3b0,err3
		b3b0 = betaQ4
		err3 = errorQ4
C     The following are from Nick's 052598 data
C     b0b0=12.1153
C     err0=.400
C     b3b0=5.6706
C     err3=.514
C     THSES ARE AVERAGE BETAY VALUES AT Q3 AND Q4 RESPECTIVELY
C     IN WHAT FOLLOWS B0,AO ARE BETA,ALPHA VALUES AT CENTER OF Q3
C     B1,A1 ARE BETA ALPHA VALUES AT EXIT OF Q3
C     B2,A2 ARE BETA ALPHA VALUES AT ENTRANCE OF Q4
C     B3,A3 ARE BETA ALPHA VALUES AT CENTER OF Q4
C     THE NONLINEAR EQUATIONS CONNECTING THESE VALUES ARE SOLVED
C     BY ITERATION
c     FIRST GUESS FOR B1,B2
		b1=b0b0
		b2=b3b0
		b1l=b1
		is=052898
		itotal=500
		if((err0.eq.0.).and.(err3.eq.0.)) itotal=1
		bet=0.
		betrms=0.
		alp=0.
		alprms=0.
		betas2=0.
		bs2rms=0.
		betas3=0.
		bs3rms=0.
c if error estimation is requested, loop over 500 randoms numbers.
		solutions = 0
		notConverged = 0
		nonPhysical = 0
		do i=1,itotal
			run=0.
			do j=1,10
				fnorm=ran(is)
				run=run+fnorm
			enddo
			b0b=b0b0+err0*(run-5.)
c			write (*,*) run-5.0
			run=0.
			do j=1,10
				fnorm=ran(is)
				run=run+fnorm
			enddo
			b3b=b3b0+err0*(run-5.)
c			write (*,*) run-5.0
			if (verbose.ne.0) then
				write(6,*) 'b0b,b3b ',b0b,b3b
			endif
c start of convergence loops
			itt=0
			ittLimit = 100
C     SOLVE FOR A1 AS FUNCTION OF B1,B2
 10		check=b1*b2-ss**2
			check1 = check
			if(check.lt.0) then
				check1=0.0
			endif
			if ( plane(1:1) .eq. 'x' ) then
				a1=(b1+dsqrt(check1))/ss
			else if ( plane(1:1) .eq. 'y' ) then
				a1=(b1-dsqrt(check1))/ss
			endif
C     SOLVE FOR A0 AS FUNCTION OF B1,A1
			a0=b1*a11*a21+a1*(1.+2.*a12*a21)+(1.+a1**2)/b1*a12*a11
C     SOLVE FOR BO AS FUNCTION OF A0,B0B
			arg = b0b**2-a12**2*(1.+a11**2)*(1.+a0**2)
			if (arg .lt. 0d0 ) then
				arg = 0d0
				if (verbose.ne.0) then
					write(6,*) 'unphysical b0b**2-a12**2*(1.+a11**2)*(1.+a0**2):', arg
				endif
			endif			
			b0=(b0b+dsqrt(arg))/(1.+a11**2)
c     SOLVE FOR B1 AS FUNCTION OF B0,A0
			b1=((b0*a11-a0*a12)**2+a12**2)/b0
			itt=itt+1
C     CHECK FOR CONVERGENCE
			diff=(b1-b1l)**2
			if((diff.lt.1.d-10).or.(itt.gt.100)) goto 20
			b1l=b1
C     SOLVE FOR A2 AS FUNCTION OF B1,A1
			a2=a1-(1.+a1**2)*ss/b1
C     SOLVE FOR A3 AS FUNCYION OF B2,A2
			a3=-b2*B11*b21+a2*(1.+2.*b12*b21)-(1.+a2**2)/b2*b12*b11
C     SOLVE FOR B3 AS FUNCTION OF B3B,A3
			temp = b3b**2-b12**2*(1.+b11**2)*(1.+a3**2)
			if ( temp .lt. 0 ) temp = 0d0
			b3 = (b3b+dsqrt(temp))/ (1.+b11**2)
C     SOLVE FOR B2 AS FUNCTION OF B3,A3
			b2=((b3*b11+a3*b12)**2+b12**2)/b3
			goto 10
 20		continue
			if (itt. gt. ittLimit) then
				notConverged = notConverged + 1
			endif
			if (check .lt. 0.0) then
				nonphysical = nonphysical + 1
			endif
			if (itt .le. ittLimit .and. check .ge. 0.0) then
C     SOLUTION HAS CONVERGED
				solutions = solutions + 1
				bs2=((b1-a1*ss2)**2+ss2**2)/b1
				bs3=((b2+a2*ss3)**2+ss3**2)/b2
				if ( source(1:2) .eq. 'AM' ) then
					bbm=((b1-a1*sbm)**2+sbm**2)/b1
					abm=a1-(1.+a1**2)*sbm/b1
				else if (source(1:2) .eq. 'BM' ) then
					bbm=((b2+a2*sbm)**2+sbm**2)/b2
					abm=a2+(1.+a2**2)*sbm/b2
				endif
c     write(6,*)'betax s2, s3 ',bs2,bs3
c     write(6,*)'betax alphax bm ',bbm,abm
				if ( verbose .ne. 0 ) then
					write(*,*) 'betaS2: ',bs2
					write(*,*) 'betaS3: ',bs3
					write(*,*) 'betaBM: ',bbm
					write(*,*) 'alphaBM: ',abm
				endif
				betas2=betas2+bs2
				bs2rms=bs2rms+bs2**2
				betas3=betas3+bs3
				bs3rms=bs3rms+bs3**2
				bet=bet+bbm
				if ( source(1:2) .eq. 'AM' ) then
					alp=alp+abm
				else if ( source(1:2) .eq. 'BM' ) then
					alp=alp-abm
				endif
				betrms=betrms+bbm**2
				alprms=alprms+abm**2
			endif
		enddo
		ffg=dfloat(solutions)
		betas2=betas2/ffg
		temp=bs2rms/ffg-betas2**2
		if(temp.lt.0.) temp=0.
		bs2rms=dsqrt(temp)

		betas3=betas3/ffg
		temp=bs3rms/ffg-betas3**2
		if(temp.lt.0.) temp=0.
		bs3rms=dsqrt(temp)

		bet=bet/ffg
		temp=betrms/ffg-bet**2
		if(temp.lt.0.) temp=0.
		betrms=dsqrt(temp)

		alp=alp/ffg
		temp=alprms/ffg-alp**2
		if(temp.lt.0.) temp=0.
		alprms=dsqrt(temp)

c		write(6,*)
c		write(6,*)
c		write(6,*) 'betaxs2 and rms            ',betas2,bs2rms
c		write(6,*) 'betaxs3 and rms            ',betas3,bs3rms
c		write(6,*) 'source point betax and rms ',bet,betrms
c		write(6,*) 'source point alphax and rms',alp,alprms
		if (nonphysical .gt. 0) then
			write(6,*) 'Non-physical solutions: ', nonphysical, '(not used in average).'
		endif
		if (notconverged .gt. 0) then
			write(6,*) 'Instances on non-convergence: ', notConverged
		endif
		write (6,*) solutions, ' valid solutions out of ', itotal, '.'
		write(*,*) 'betaS2: ',betas2, bs2rms
		write(*,*) 'betaS3: ',betas3, bs3rms
		write(*,*) 'beta: ',bet, betrms
		write(*,*) 'alpha: ',alp, alprms
		stop
		end
