       program raspic
C===================================================================
C Program to analyse RASNIK data.
C
C First the muon object files are read in. From these files a
C prediction of the RASNIK readings is made. These are then
C subtracted from the measured values to get calibration values.
C (chamber sags from parameter files)
C
C The calibrated RASNIK data is then used in a fitting procedure to
C obtain the BML and BOL (pseudo) geometry. 
C The geometry is usually translated in a sagitta correction on
C the BML, which makes comparison with muons more easy.
C
C Important routines are:
C    cgeom  : from geometry to objectfile
C    initras: read objectfile, get RASNIK positions and rotations.
C    preras : predict RASNIK readings.
C
C Marcel Vreeswijk 1/98
C=================================================================== 
      include 'mcommon'
            double precision xrascal(100,4,4),yrascal(100,4,4),
     +                  rotrascal(100,4,4),
     +                  dmrascal(100,4,4),sscal(4,4),sscals(4,4),
     +    xcals(4,4),ycals(4,4),dmcals(4,4),rotcals(4,4)
            double precision   ax,ay,az,xin,yin,dmin,rotin 
	double precision xuse,yuse,zuse,tfits(50),
     +        xerr,yerr,dmerr,roterr,dsave,chix,chiz,chim,chir,
     +        esave
        double precision a,b,c,an,bn,cn,mat(3,3),dn(3),dnn(3)

      double precision rbil(3),rbml(3),rbol(3),
     +  xx1,xx2,xx3,yy1,yy2,yy3,zz1,zz2,zz3
      real sag

        integer iwork(9),ifail
      character*80 cfile(20)
      character*10 system(4),sys(4),sysu(4)
      integer i,j,jj,i1,ibingo
      integer irunfit,isys,id,icalread,idirty,k
      common /pawc/hmem
      real hmem(500000)
      character*4 subname
      character*4 sub4name
      character*8 sub8name
      character*10 sub10name
      character*250 linec
    
c     data iorder /2185,2187,2188,2189,2015,2016,2017,2018, 
c    +             2019,2020,2021,2022,2152,2159,2160,2161,84*0/
c     data isuffix /2185,2187,2188,2189,2015,2016,2017,2018, 
c    +             2019,2020,2021,2022,2152,2159,2160,2161,84*0/

      data iorder /2199,2200,2201,9999,96*0/
      data isuffix /2199,2200,2201,9999,96*0/

c      data iorder /2011,2012,2013,2014,2015,2016,2017,2018, 
c     +             2019,2020,2021,2022,2152,2159,2160,2161,84*0/
c      data isuffix /2011,2012,2013,2014,2015,2016,2017,2018, 
c     +             2019,2020,2021,2022,2152,2159,2160,2161,84*0/



      debug=1

      sys(1)='BIL'
      sys(2)='BML'
      sys(3)='BOL'
      sys(4)='PRO'
      sysu(1)='bil'
      sysu(2)='bml'
      sysu(3)='bol'
      sysu(4)='pro'
      system(1)='Q0'
      system(2)='P0'
      system(3)='Q1'
      system(4)='P1'
      
      call mninit(5,6,7)

      do isys=1,4
         do j=1,50
            do jco=1,4
               offset(isys,jco,j)=0
            enddo
         enddo
      enddo
C calibration scheme...
C 1) set rotations of masks
C 2) set rotations of CCDs, average of two mask readings
C 3) correct rotation of masks, becuae average was used.
C 4) offset x,y,Mag.
C 
C set rotation angles PRO mask
      offset(4,1,5)=0.011 *0.  ! Q0
      offset(4,2,5)=0.030 *0.  ! P0
      offset(4,3,5)=0.007 *0.  ! Q1
      offset(4,4,5)=-0.005*0.  ! P1
C set rotation angles BIL mask
      offset(1,1,5)=0.00  ! BIL0
      offset(1,2,5)=0.00  ! BIL1
      offset(1,3,5)=0.00  ! BIL2
      offset(1,4,5)=0.00  ! BIL3
C BML
      offset(2,1,5)=0.00  ! BML0
      offset(2,2,5)=0.00 ! BML1
      offset(2,3,5)=0.00  ! BML2
      offset(2,4,5)=0.00  ! BML3
C BOL
      offset(3,1,5)=-0.00 ! BOL0
      offset(3,2,5)=0.0  ! BOL1
      offset(3,3,5)=-0.00 ! BOL2
      offset(3,4,5)=0.0  ! BOL3
C--
      jco=1
      call hlimit(500000)
      do j=1,4
      do isys=1,4
         id=isys*100
         call hbook1(id+j,'xcal  ',250, -0.5, 0.5, 0.)
         call hbook1(id+10+j,'ycal  ',250, -0.5, 0.5, 0.)
         call hbook1(id+20+j,'rotcal',250, -0.01, 0.01, 0.)
         call hbook1(id+30+j,'dmcal ',250, -0.005, 0.005, 0.)
      enddo
      enddo
      
      call hbook1(141,'sagbml',200,1.2, 1.8, 0.)
      call hbook1(142,'sagbol',200,1.5, 2.5, 0.)
      

      stf=sin( 25.d0/180.d0*acos(-1.d0) )
      ctf=cos( 25.d0/180.d0*acos(-1.d0) )
      
      ycham(1)=4883.0
      ycham(2)=7543.000
      ycham(3)=10951.000
      zcham(1)=-2471.286
      zcham(2)=-3841.714
      zcham(3)=-5545.000
      xcham(1)=2530
      xcham(2)=3920
      xcham(3)=5620
      dzcham(1)=960.
      dzcham(2)=1440.
      dzcham(3)=2160.



      do i=1,20
         ioptions(i)=0.
      enddo
C==================================================


      open(30,file='steer')
      read(30,*)irunfit
      close(30)
C****************** O P T I O N S *****************************
      ioptions(3)=1.            ! set all step BOL pars to zero
      ioptions(6)=2.            ! rot frame
      ioptions(5)=1.            ! sag
      ioptions(4)=-1.           ! special CP treatment in shift
ccc      ioptions(4)=1.           ! For MArtins test
      nruns=3                   ! 'nruns'  = # muons runs
C--
C            :  'iorder' = muon run number array
C            :  'isuffix'= muon run file suffix for initras
C--
C**************************************************************

C some stuff for frame      
      call torot(0.d0,ycham(1),zcham(1),
     +     xchamp(1),ychamp(1),zchamp(1))
      call torot(0.d0,ycham(2),zcham(2),
     +     xchamp(2),ychamp(2),zchamp(2))
      call torot(0.d0,ycham(3),zcham(3),
     +     xchamp(3),ychamp(3),zchamp(3))
      
C********* get RASNIK input **************************************
C get also 'igo', i.e. the index of the run to be fitted
      irun=100
      do j=1,irun
         if (iorder(j).eq.0) goto 61
         i=j
         write(sub4name,'(I4)')iorder(j)
         sub8name=sub4name//'.off'
         cfile(j)='run'//sub8name
         print *,'input file ',cfile(j)
         open(32,file=cfile(j))
         read(32,'(a)')linec
         run(j)=iorder(j)
         if (run(i).eq.irunfit) igo=i 

         do i2=1,16
            read(32,*,ERR=60,END=60)sub8name,
     +           xin,yin,dmin,ax,ay,rotin
            isys=4
            do i1=1,4
               if (sub8name(1:3).eq.sysu(i1)) then
                  isys=i1
                  read(sub8name(4:4),'(I1)')ityp
                  ityp=ityp+1
               endif
               if (sub8name(1:2).eq.system(i1)) ityp=i1
            enddo
            xrasin(i,isys,ityp)=xin
            yrasin(i,isys,ityp)=yin
            dmrasin(i,isys,ityp)=dmin
            rotrasin(i,isys,ityp)=rotin
         enddo
         do isys=1,4
         do i1=1,4
            exrasin(i,isys,i1)=0.010
            eyrasin(i,isys,i1)=0.010
            edmrasin(i,isys,i1)=0.0002
            erotrasin(i,isys,i1)=0.00002
         enddo
         enddo
      enddo                     ! over nruns
      goto 61
 60   print *,'FATAL RASNIK INPUT'
 61   continue

*      irun=j-1
      print *,'RASNIKS ',irun
      if (igo.eq.0) print *,'FATAL: fit run not found '
C********* end get RASNIK input *********************************

C************* PREDICTION and CALIBRATION on object files *******
C--
C set by hand: 'nruns'  = # muons runs
C            :  argument of initras(iobj-file)
C            :  'iorder' = muon run number array
C--
      print *, 'CALIBRATION RUNS ',nruns
      sagbmlmean=0.
      sagbolmean=0.

      do idirty=1,2


      do j=1,nruns
      do isys=1,4
      do jj=1,4
         sscal(isys,jj)=0.
         xcal(isys,jj)=0.
         ycal(isys,jj)=0.
         rotcal(isys,jj)=0.
         dmcal(isys,jj)=0.
         sscals(isys,jj)=0.
         xcals(isys,jj)=0.
         ycals(isys,jj)=0.
         rotcals(isys,jj)=0.
         dmcals(isys,jj)=0.
         xrascal(j,isys,jj)=0.
         yrascal(j,isys,jj)=0.
         rotrascal(j,isys,jj)=0.
         dmrascal(j,isys,jj)=0.
      enddo
      enddo
      enddo

      do i=1,50
         rfits(i)=0.
      enddo

      do j=1,nruns
C parameter files for sagchambers
         write(subname,'(I4)')isuffix(j)
         cfile(j)='../lini/para.'//subname
         open(35,file=cfile(j))
         do i=1,50
            read(35,*)tfits(i)
            rfits(i)=rfits(i)+tfits(i)/nruns
            rfitssave(i)=rfits(i)
         enddo
         close(35)
         call hfill(141,real(rfits(12)),0.,1.)
         call hfill(142,real(rfits(13)),0.,1.)
         print *,'============PREDICTION================'
         print *,'result for muon run ',iorder(j)
         call initras(isuffix(j))
         do isys=1,4
            call predict(isys)
            do jj=1,4
               write(*,2000)sys(isys),jj,system(jj),
     +              xras(jj),yras(jj),rotras(jj),dmras(jj)
            enddo
         enddo
         print *,'============================================'
      enddo

c         rfits(22)=(rfitssave(22)*0.906/0.423+rfitssave(23))
c     +           /(0.906**2/0.423+0.423)
c         rfits(23)=(rfitssave(22) - rfitssave(23)*0.906/0.423)
c     +           /(-0.423-0.906**2/0.423)
c         rfits(22)=rfitssave(22)*0.906 - rfitssave(23)*0.423
c         rfits(23)=rfitssave(22)*0.423 + rfitssave(23)*0.906


      open(21,file='dmtest.dat')
      open(22,file='xtest.dat')
      open(23,file='ytest.dat')
      open(24,file='rottest.dat')
        do j=1,nruns
         ibingo=0
         do i1=1,irun
            if (run(i1).eq.iorder(j)) ibingo=i1
         enddo
         if (ibingo.eq.0) print *,' NONSENS CALIBRATION '
         print *,'================CALIBRATION================='
         print *,'result for muon run ',iorder(j)
         call initras(isuffix(j))
         do isys=1,4
         call predict(isys)
         do jj=1,4
C get run by run calibration values
            xrascal(j,isys,jj)=xras(jj)-xrasin(ibingo,isys,jj)
            yrascal(j,isys,jj)=yras(jj)-yrasin(ibingo,isys,jj)
            rotrascal(j,isys,jj)=rotras(jj)-rotrasin(ibingo,isys,jj)
            dmrascal(j,isys,jj)=dmras(jj)-dmrasin(ibingo,isys,jj)
C prepare calibration
C
         thet=-117.*3.1415/180.     
        ss=-xrascal(j,isys,jj)*sin(thet)+yrascal(j,isys,jj)*cos(thet)
        if (isys.eq.4 .and. (j.eq.1 .or. j.eq.3)) then
          ss= 0.99240*xrascal(j,isys,jj) -0.12301*yrascal(j,isys,jj)
        elseif (isys.eq.4 .and. (j.eq.2 .or. j.eq.4)) then
          ss= -0.99544*xrascal(j,isys,jj) -0.09531*yrascal(j,isys,jj)
        endif
         sscal(isys,jj)=sscal(isys,jj)+ss
         sscals(isys,jj)=sscals(isys,jj)+ss**2
C
            xcal(isys,jj)=xcal(isys,jj)+xrascal(j,isys,jj)
            ycal(isys,jj)=ycal(isys,jj)+yrascal(j,isys,jj)
            rotcal(isys,jj)=rotcal(isys,jj)+rotrascal(j,isys,jj)
            dmcal(isys,jj)=dmcal(isys,jj)+dmrascal(j,isys,jj)
            xcals(isys,jj)=xcals(isys,jj)+xrascal(j,isys,jj)**2
            ycals(isys,jj)=ycals(isys,jj)+yrascal(j,isys,jj)**2
            rotcals(isys,jj)=rotcals(isys,jj)+
     +                       rotrascal(j,isys,jj)**2
            dmcals(isys,jj)=dmcals(isys,jj)+dmrascal(j,isys,jj)**2
            write(*,2000)sys(isys),jj,system(jj),
     +           xrascal(j,isys,jj),
     +           yrascal(j,isys,jj),rotrascal(j,isys,jj),
     +           dmrascal(j,isys,jj)
         enddo
         if (isys.eq.4) then
         write(21,*)real(dmrascal(j,isys,1)),
     +              real(dmrascal(j,isys,2)),
     +              real(dmrascal(j,isys,3)),
     +              real(dmrascal(j,isys,4)),
     +              real(dmrasin(j,isys,1)),
     +              real(dmrasin(j,isys,2)),
     +              real(dmrasin(j,isys,3)),
     +              real(dmrasin(j,isys,4)),j
         write(22,*)real(xrascal(j,isys,1)),
     +              real(xrascal(j,isys,2)),
     +              real(xrascal(j,isys,3)),
     +              real(xrascal(j,isys,4)),
     +              real(xrasin(j,isys,1)),
     +              real(xrasin(j,isys,2)),
     +              real(xrasin(j,isys,3)),
     +              real(xrasin(j,isys,4)),j
         write(23,*)real(yrascal(j,isys,1)),
     +              real(yrascal(j,isys,2)),
     +              real(yrascal(j,isys,3)),
     +              real(yrascal(j,isys,4)),
     +              real(yrasin(j,isys,1)),
     +              real(yrasin(j,isys,2)),
     +              real(yrasin(j,isys,3)),
     +              real(yrasin(j,isys,4)),j
         write(24,*)real(rotrascal(j,isys,1)),
     +              real(rotrascal(j,isys,2)),
     +              real(rotrascal(j,isys,3)),
     +              real(rotrascal(j,isys,4)),
     +              real(rotrasin(j,isys,1)),
     +              real(rotrasin(j,isys,2)),
     +              real(rotrasin(j,isys,3)),
     +              real(rotrasin(j,isys,4)),j

         endif
         print *,'============================================'
      enddo
      enddo

      close(21)
      close(22)
      close(23)
      close(24)

      print *,' CAL SAG BML ',sagbmlmean
      print *,' CAL SAG BOL ',sagbolmean
      do isys=1,4
C calibration values
      do jj=1,4
         sscal(isys,jj)=sscal(isys,jj)/nruns
         xcal(isys,jj)=xcal(isys,jj)/nruns
         ycal(isys,jj)=ycal(isys,jj)/nruns
         rotcal(isys,jj)=rotcal(isys,jj)/nruns
         dmcal(isys,jj)=dmcal(isys,jj)/nruns
         sscals(isys,jj)=sscals(isys,jj)/nruns
         xcals(isys,jj)=xcals(isys,jj)/nruns
         ycals(isys,jj)=ycals(isys,jj)/nruns
         rotcals(isys,jj)=rotcals(isys,jj)/nruns
         dmcals(isys,jj)=dmcals(isys,jj)/nruns
         sserr=sqrt(sscals(isys,jj)-sscal(isys,jj)**2)
         xerr=sqrt(xcals(isys,jj)-xcal(isys,jj)**2)
         yerr=sqrt(ycals(isys,jj)-ycal(isys,jj)**2)
         dmerr=sqrt(dmcals(isys,jj)-dmcal(isys,jj)**2)
         roterr=sqrt(rotcals(isys,jj)-rotcal(isys,jj)**2)
          write(*,2003)sys(isys),jj,
     +            sscal(isys,jj),sserr,
     +            xcal(isys,jj),xerr,
     +             ycal(isys,jj),yerr,
     +             rotcal(isys,jj),roterr,
     +             dmcal(isys,jj),dmerr
      enddo
      enddo
C fill soem histoos
      do j=1,nruns
         do isys=1,4
            do jj=1,4
               id=isys*100
               call hfill(id+jj,
     +              real(xcal(isys,jj)-xrascal(j,isys,jj)),0.,1.)
               call hfill(id+10+jj,
     +              real(ycal(isys,jj)-yrascal(j,isys,jj)),0.,1.)
               call hfill(id+30+jj,
     +              real(dmcal(isys,jj)-dmrascal(j,isys,jj)),0.,1.)
               call hfill(id+20+jj,
     +              real(rotcal(isys,jj)-rotrascal(j,isys,jj)),0.,1.)
            enddo
         enddo
      enddo

C--------------------------------------------------------------
      if (idirty.eq.1) then
c     set CCD rotation. Could have been ignored, since it only has
c     a relative meaning. Active variable.
c     
         
         
C-- skip this to use Martins variables, note that rotcal is used in Martins 
C   case
	goto 78
         do isys=1,3
            do i=1,2
C     set rotation angles CCD,in-plane thus 1=3 and 2=4 
            offset(isys,i,4)=(rotcal(isys,i)+rotcal(isys,i+2))/2
            offset(isys,i+2,4)=(rotcal(isys,i)+rotcal(isys,i+2))/2
C then correct mask again
            offset(isys,i,5)=-(rotcal(isys,i)-
     +           (rotcal(isys,i)+rotcal(isys,i+2))/2)
            offset(isys,i+2,5)=-(rotcal(isys,i+2)-
     +           (rotcal(isys,i)+rotcal(isys,i+2))/2)
            print *,'CCD cal sys i ',isys,i,offset(isys,i,4)
            print *,'CCD cal sys i ',isys,i+2,offset(isys,i+2,4)
         enddo
      enddo
      
      
      do isys=4,4
         do i=1,4
C     set rotation angles CCDs 
            offset(isys,i,4)=rotcal(isys,i)
            print *,'CCD cal sys i ',isys,i,offset(isys,i,4)
         enddo
      enddo
 78	continue

	if (idirty.eq.1) then
	        do ii=1,4
        do i=1,4
          print *,'MARCELs ',xcal(ii,i),ycal(ii,i),rotcal(ii,i)
        enddo
        enddo
	endif
      endif ! idirty
      enddo  ! idirty, needed a double loop to calibrate, see comments above
C     
C     
C---------------------------------------------------------------
C
        GOTO 79 ! SKIP
        open(34,file='table.marcel.bil')
        do i=1,4
          write(34,*)-xcal(1,i)
          write(34,*)-ycal(1,i)
          write(34,*)-rotcal(1,i)*1000.
        enddo
        close(34)

        open(34,file='table.marcel.bml')
        do i=1,4
          write(34,*)-xcal(2,i)
          write(34,*)-ycal(2,i)
          write(34,*)-rotcal(2,i)*1000.
        enddo
        close(34)

        open(34,file='table.marcel.bol')
        do i=1,4
          write(34,*)-xcal(3,i)
          write(34,*)-ycal(3,i)
          write(34,*)-rotcal(3,i)*1000.
        enddo
        close(34)

         open(34,file='table.marcel.proj')
         do i=1,4
           write(34,*)-xcal(4,i)
           write(34,*)-ycal(4,i)
           write(34,*)-rotcal(4,i)*1000.
         enddo
         close(34)

C--

 	open(34,file='table.bil')
 	do i=1,4
 	  read(34,*)xcal(1,i)
 	  read(34,*)ycal(1,i)
 	  read(34,*)rotcal(1,i)
        enddo
        close(34)

        open(34,file='table.bml')
        do i=1,4
          read(34,*)xcal(2,i)
          read(34,*)ycal(2,i)
          read(34,*)rotcal(2,i)
        enddo
        close(34)

        open(34,file='table.bol')
        do i=1,4
          read(34,*)xcal(3,i)
          read(34,*)ycal(3,i)
          read(34,*)rotcal(3,i)
        enddo
        close(34)

         open(34,file='table.proj')
         do i=1,4
           read(34,*)xcal(4,i)
           read(34,*)ycal(4,i)
           read(34,*)rotcal(4,i)
         enddo
         close(34)

	do ii=1,4
	do i=1,4
	  xcal(ii,i)=-xcal(ii,i)
	  ycal(ii,i)=-ycal(ii,i)
	  rotcal(ii,i)=-rotcal(ii,i)/1000.
	  print *,'MARTINs ',xcal(ii,i),ycal(ii,i),rotcal(ii,i)
	enddo
	enddo
	  
 79     CONTINUE

C!!! put in LAB VALUES FOR BOL
 
c        xcal(3,1)=-5.082
c        ycal(3,1)=-10.321 
c        xcal(3,2)=-8.118
c        ycal(3,2)=-13.042
c        xcal(3,3)=-5.214
c        ycal(3,3)=-11.915
c        xcal(3,4)=-8.680
c        ycal(3,4)=-13.591
c        rotcal(3,1)=12.99/1000.
c        rotcal(3,2)= 8.44/1000.
c        rotcal(3,3)=14.01/1000.
c        rotcal(3,4)=11.89/1000.
        
	
C calibrate RASNIK input with the muon calibration values
      do i1=1,irun
         do isys=1,4
         do i=1,4
C take care of passive variables.
            xrasin(i1,isys,i)=xrasin(i1,isys,i)+xcal(isys,i)
            yrasin(i1,isys,i)=yrasin(i1,isys,i)+ycal(isys,i)
            dmrasin(i1,isys,i)=dmrasin(i1,isys,i)+dmcal(isys,i)
C-- this line only in Martins case
             rotrasin(i1,isys,i)=rotrasin(i1,isys,i)+rotcal(isys,i)
         enddo
      enddo
      enddo

C print calibration
      do isys=1,4
C calibration values
         print *,'tex output '
         thet=-117.*3.1415/180.     
        
         print *,' xpos ypos sag mag rotCCD rotMSK '
      do jj=1,4
         ss= -xcal(isys,jj)*sin(thet)+ycal(isys,jj)*cos(thet)
	if (isys.eq.4 .and. (j.eq.1 .or. j.eq.3)) then
	  ss= 0.99240*xcal(isys,jj) -0.12301*ycal(isys,jj)
        elseif (isys.eq.4 .and. (j.eq.2 .or. j.eq.4)) then
	  ss= -0.99544*xcal(isys,jj) -0.09531*ycal(isys,jj)
        endif
         write(*,2001)sys(isys),jj,xcal(isys,jj),
     +                             ycal(isys,jj),ss,
     +                  dmcal(isys,jj),
     +                 offset(isys,jj,4),offset(isys,jj,5)
      enddo
      enddo


Ctest calibration, 
      PRINT *,'TEST CALIBRATION '
      j=1
      call initras(isuffix(j))
      do isys=1,4
         call predict(isys)
         do jj=1,4
C     get run by run calibration values
            xrascal(j,isys,jj)=xras(jj)-xrasin(ibingo,isys,jj)
            yrascal(j,isys,jj)=yras(jj)-yrasin(ibingo,isys,jj)
            rotrascal(j,isys,jj)=rotras(jj)-rotrasin(ibingo,isys,jj)
            dmrascal(j,isys,jj)=dmras(jj)-dmrasin(ibingo,isys,jj)
            write(*,2000)sys(isys),jj,system(jj),
     +           xrascal(j,isys,jj),
     +           yrascal(j,isys,jj),rotrascal(j,isys,jj),
     +           dmrascal(j,isys,jj)            
         enddo
         print *,'============================================'
      enddo
C     endtest
      
 2000 format(A3,1X,I3,1X,A2,1X,'X: ',F8.4,'  Y: ',F8.4,
     +                 '  ROT: ',F10.6,'  M: ',F9.5)
 2003 format(A3,'&',I2,'&',F7.3,'&',F7.3,'&',F7.3,'&',F7.3,
     +        '&',F7.3,
     +                   '&',F7.3,
     +                 '&',F9.5,'&',F9.5,'&',F7.5,'&',F7.5,'\\')
 2001 format(A3,'&',I2,'&',F7.3,'&',F7.3,F7.3,
     +                 '&',F9.5,'&',F7.5,'&',F7.5,'\\')

C===============================================================

C test for non projective tracks
c      do i=1,19
c         rfits(i)=0.
c      enddo
c        rfits(19)=rfits(19)+0.002  ! ax_bol
c        rfits(22)=rfits(22)+0.001  ! ay_bol
c        rfits(23)=rfits(23)+0.001  ! az_bol
      
C a visible rotation
c      rfits(22)=rfits(22)+0.906*0.002
c      rfits(23)=rfits(23)+0.423*0.002
C not visible
c      rfits(22)=rfits(22)-0.423*0.002
c      rfits(23)=rfits(23)+0.906*0.002

c      call torot(1.d0,0.d0,0.d0,a,b,c)
c      call torot(1.d0,0.001d0,0.d0,an,bn,cn) ! rotation y-axis

c      print *,a,b,c
c      print *,an,bn,cn
c      mat(1,1)=0.
c      mat(1,2)=c
c      mat(1,3)=-b
c      mat(2,1)=-c
c      mat(2,2)=0.
c      mat(2,3)=a
c      mat(3,1)=-b
c      mat(3,2)=a
c c     mat(3,3)=0
c c     dn(1)=an-a
c      dn(2)=bn-b
c      dn(3)=cn-c
c      print *,'voor ',dn
c      dnn(1)=0
c      DNN(2)=0c
c      dnn(3)=0
c      call DEQN(3,MAT,3,Iwork,IFAIL,1,dn)
c      if (ifail.ne.0) print *,'MATRIX failes '
c      mat(1,1)=0.
c      mat(1,2)=c
c      mat(1,3)=-b
c c     mat(2,1)=-c
c c     mat(2,2)=0.
c      mat(2,3)=a
c      mat(3,1)=-b
c      mat(3,2)=a
c      mat(3,3)=0
c      do i=1,3
c         do j=1,3
c            dnn(i)=dnn(i)+mat(i,j)*dn(j)
c         enddo
c      enddo
c      print *,'after',dnn
c      stop
      print *,'RUN TO BE FITTED ',irunfit,run(igo)
C fitting part
      
      do i=1,29
         print *,i,rfits(i)
      enddo

      isys=4
      call cgeom(-1)
      call initras(-1)
      call predict(isys)
      chi2=0.d0
      chix=0.d0
      chiz=0.d0
      chim=0.d0
      chir=0.d0
      do i=1,4
         chix=chix+ (xras(i)-xrasin(igo,isys,i))**2 / 
     +                exrasin(igo,isys,i)**2
         chiz=chiz+ (yras(i)-yrasin(igo,isys,i))**2 / 
     +                eyrasin(igo,isys,i)**2
         chim=chim+ (dmras(i)-dmrasin(igo,isys,i))**2 /
     +                edmrasin(igo,isys,i)**2
         chir=chir+ (rotras(i)-rotrasin(igo,isys,i))**2 /
     +                erotrasin(igo,isys,i)**2
      enddo
      write(*,*)'test chi2 ',isys,real(chix),
     +           real(chiz),real(chim),real(chir)



      ioptions(1)=0 ! set to 1, MINOS is called
      ifs=1 ! BIL
       call fitfcn
      ifs=2 ! BML
       call fitfcn
      ifs=3 ! BOL
       call fitfcn
      ifs=4 ! PRO 
      ioptions(1)=0 ! set to 1, MINOS is called
       call fitfcn

      goto 77  ! comment out if you do not want alternative correction

      do i=1,50
         rfitssave(i)=rfits(i)
      enddo
      call mysag
      call sag2dnew
      do i=1,50
         rfits(i)=rfitssave(i)
      enddo


 77   continue

C fitting is done, now histoos and all that.
      call cgeom(-1)
      call initras(-1)
c 
      ioptions(4)=2 ! set to 2, wire sags included (track mode)
      print *,rfits
      call getsagittag(sag0,0)
      call getsagittag(sag1,1)
      print *,'sag0 sag1 dsag ',sag0,sag1,sag1-sag0
      call sag2d
      ioptions(4)=-1 ! changed back to -1 (component mode)
      
      open(38,file='rasnik.dat')
      do i=1,50
         write(38,*)rfits(i)
      enddo
      close(38)
C histo out
      lun  = 25
      CALL HROPEN(lun,'oops','uit','N',1024,ISTAT)
      if (istat.ne.0) then
        CALL HROPEN(lun,'oops','uit','U',1024,ISTAT)
      endif
      call hrout(0,icycle,'T')
      call hrend('oops')
      close(lun)
      end

      subroutine newsag(posbil,posbml,posbol,rsag)
      implicit none
      include 'mcommon'
      integer i,i1,i2,kcp,isys,i3
      double precision theta,LL,mat(3,3),
     +                 vec(3),
     +                 ex(3),exa(3),exaa(3),dy,dz,sag(4)
      double precision xnom,ynom,znom,xgbml,ygbml,zgbml
      double precision posbil(3),posbml(3),posbol(3),dsag
      real rsag,fl
      double precision xp,yp,
     + xbil,ybil,zbil,xbml,ybml,zbml,xbol,ybol,zbol,
     + daa,dbb,a,b,c,d,xgnom(4),ygnom(4),zgnom(4)

      isys=4
      do i=1,50
         rfitssave(i)=rfits(i)
         rfits(i)=0.
      enddo
      call cgeom(-1)
      call initras(-1)
      call predict(isys)
      
      do i=1,4
c     print *,'dmras ',dmrasin(igo,isys,i)
         xbml=dmrasin(igo,isys,i)/(1.+dmrasin(igo,isys,i))
     +        * xrasin(igo,isys,i)
         ybml=dmrasin(igo,isys,i)/(1.+dmrasin(igo,isys,i))
     +        * yrasin(igo,isys,i)
         
         LL=sqrt(
     +        (graspos(icp(isys,1,i),1)-graspos(icp(isys,3,i),1))**2+
     +        (graspos(icp(isys,1,i),2)-graspos(icp(isys,3,i),2))**2+
     +        (graspos(icp(isys,1,i),3)-graspos(icp(isys,3,i),3))**2 )
         zbml=(1.-dmrasin(igo,isys,i)) /
     +        (1.+dmrasin(igo,isys,i))*LL
         
         dy=graspos(icp(isys,3,i),2)-graspos(icp(isys,2,i),2)
         dz=graspos(icp(isys,3,i),3)-graspos(icp(isys,2,i),3)
         theta=atan2(dy,dz)
         
         
         zbml=0.
         
         ex(1)=xbml
         ex(2)=ybml
         ex(3)=zbml
         
C     goto component frame

        call torotras(offset(isys,i,5),ex(1),ex(2),xp,yp)
        ex(1)=xp
        ex(2)=yp
         do i1=1,3
            exa(i1)=0.
            do i2=1,3
               exa(i1)=exa(i1)+ras(icp(isys,1,i),i2,i1)*ex(i2)
            enddo
         enddo
C     goto global frame
         do i1=1,3
            exaa(i1)=0.
            do i2=1,3
               kcp=rastocp(icp(isys,1,i))
               exaa(i1)=exaa(i1)+cp(kcp,i2,i1)*exa(i2)                   
            enddo
            exaa(i1)=exa(i1)
         enddo
         
         sag(i)=         0.*exaa(1)+
     +        cos(theta)*exaa(2)+
     +        -sin(theta)*exaa(3)
c         print *,'sag ind ',sag(i)

         xgnom(i)=graspos(icp(isys,2,i),1)
         ygnom(i)=graspos(icp(isys,2,i),2)
         zgnom(i)=graspos(icp(isys,2,i),3)


      enddo
         
      print *,'newsag direct ',(sag(1)+sag(2)+sag(3)+sag(4))/4.


      xbil=posbil(1)
      ybil=posbil(2)
      zbil=posbil(3)
      xbml=posbml(1)
      ybml=posbml(2)
      zbml=posbml(3)
      xbol=posbol(1)
      ybol=posbol(2)
      zbol=posbol(3)

      daa=(xbml-xgnom(2))/(xgnom(4)-xgnom(2))
      dbb=(zbml-zgnom(1))/(zgnom(2)-zgnom(1))
      a=sag(1)
      c=sag(2)
      b=sag(3)
      d=sag(4)
      call fpint(dsag,daa,dbb,a,b,c,d)
      rsag=dsag


      print *,'sag interp ',dsag
      print *,'daa dbb ',daa,dbb

      do i=1,50
         rfits(i)=rfitssave(i)
      enddo
      end










      subroutine mysag
      implicit none
      include 'mcommon'
      integer i,i1,i2,kcp,isys,i3,iav,k
      double precision theta,xbml,ybml,zbml,LL,mat(3,3),
     +                 vec(3),
     +                 ex(3),exa(3),exaa(3),dy,dz,sag(4)
      double precision xgbml,ygbml,zgbml,xnom,ynom,znom,
     + xgmsk,ygmsk,zgmsk,dm,a,b,c,
     + xgccd,ygccd,zgccd,distx,distz,
     + xglen(4),yglen(4),zglen(4),
     + xgnom(4),ygnom(4),zgnom(4),xp,yp,xcom,wxv,wxl,rsave(50)
      double precision xx1,xx2,xx3,yy1,yy2,yy3,zz1,zz2,zz3
      double precision xbil,xbol,ybil,ybol,zbil,zbol,Lv,Lb
        real fl
      do i=1,50
         rsave(i)=rfits(i)
c         if (i.ge.10 .and. i.le.18) rfits(i)=0.
      enddo
C     INPLANE TO ZERO
c      rfits(11)=0.
c      rfits(12)=0.
c      rfits(17)=0.
c      rfits(18)=0.

C optionally, pending below
c      rfits(10)=0.
c      rfits(13)=0.
c      rfits(14)=0.
c      rfits(15)=0.
c      rfits(16)=0.
      isys=4

C to get focal le
CMV	do i=1,50
CMV	   rfits(i)=0.
CMV	enddo
C end          
      call cgeom(-1)
      call initras(-1)
      call predict(isys)

      do i=1,50
         rfits(i)=rsave(i)
      enddo
      
      do i=1,4
C direct calculation
         xbml=dmrasin(igo,isys,i)/(1.+dmrasin(igo,isys,i))
     +        * xrasin(igo,isys,i)
         ybml=dmrasin(igo,isys,i)/(1.+dmrasin(igo,isys,i))
     +        * yrasin(igo,isys,i)
         
         LL=sqrt(
     +        (graspos(icp(isys,1,i),1)-graspos(icp(isys,3,i),1))**2+
     +        (graspos(icp(isys,1,i),2)-graspos(icp(isys,3,i),2))**2+
     +        (graspos(icp(isys,1,i),3)-graspos(icp(isys,3,i),3))**2 )
         zbml=(1.-dmrasin(igo,isys,i)) /
     +        (1.+dmrasin(igo,isys,i))*LL
        
         dy=graspos(icp(isys,3,i),2)-graspos(icp(isys,2,i),2)
         dz=graspos(icp(isys,3,i),3)-graspos(icp(isys,2,i),3)
         theta=atan2(dy,dz)
Cend direct calculation
         
         xnom=graspos(icp(isys,2,i),1)
         ynom=graspos(icp(isys,2,i),2)
         znom=graspos(icp(isys,2,i),3)
         
         ex(1)=xrasin(igo,isys,i)
         ex(2)=yrasin(igo,isys,i)
         ex(3)=0.
   
C     goto component frame
        call torotras(offset(isys,i,5),ex(1),ex(2),xp,yp)
        ex(1)=xp
        ex(2)=yp

         do i1=1,3
            exa(i1)=0.
            do i2=1,3
               exa(i1)=exa(i1)+ras(icp(isys,1,i),i2,i1)*ex(i2)
            enddo
         enddo
C     goto global frame
         do i1=1,3
            exaa(i1)=0.
            do i2=1,3
               kcp=rastocp(icp(isys,1,i))
               exaa(i1)=exaa(i1)+cp(kcp,i2,i1)*exa(i2)                   
            enddo
            exaa(i1)=exa(i1)
         enddo

         xgmsk=exaa(1)+graspos(icp(isys,1,i),1)
         ygmsk=exaa(2)+graspos(icp(isys,1,i),2)
         zgmsk=exaa(3)+graspos(icp(isys,1,i),3)
         
         xgccd=graspos(icp(isys,3,i),1)
         ygccd=graspos(icp(isys,3,i),2)
         zgccd=graspos(icp(isys,3,i),3)
         

c normal            dm=dmrasin(igo,isys,i)
c test
         Lv=sqrt(
     +        (graspos(icp(isys,1,i),1)-graspos(icp(isys,2,i),1))**2+
     +        (graspos(icp(isys,1,i),2)-graspos(icp(isys,2,i),2))**2+
     +        (graspos(icp(isys,1,i),3)-graspos(icp(isys,2,i),3))**2 )
         Lb=sqrt(
     +        (graspos(icp(isys,2,i),1)-graspos(icp(isys,3,i),1))**2+
     +        (graspos(icp(isys,2,i),2)-graspos(icp(isys,3,i),2))**2+
     +        (graspos(icp(isys,2,i),3)-graspos(icp(isys,3,i),3))**2 )

	fl=1./Lb+1./Lv
        fl=1./fl
c	print *,'focal len. ',isys,i,real(Lv),real(Lb),fl

         dm=Lb/Lv
         xglen(i)=xgmsk+ (xgccd-xgmsk)
     +      /(1.+dm)
         yglen(i)=ygmsk+ (ygccd-ygmsk)
     +      /(1.+dm)
         zglen(i)=zgmsk+ (zgccd-zgmsk)
     +      /(1.+dm)

         xgnom(i)=graspos(icp(isys,2,i),1)
         ygnom(i)=graspos(icp(isys,2,i),2)
         zgnom(i)=graspos(icp(isys,2,i),3)
C--
         call distxz( graspos(icp(isys,1,i),1),
     +                graspos(icp(isys,1,i),2),
     +                graspos(icp(isys,1,i),3),
     +                graspos(icp(isys,2,i),1),
     +                graspos(icp(isys,2,i),2),
     +                graspos(icp(isys,2,i),3),
     +                graspos(icp(isys,3,i),1),
     +                graspos(icp(isys,3,i),2),
     +                graspos(icp(isys,3,i),3),
     +                distx,distz)
         print *,'old distx distz ',distx,distz
C--
C--
         call distxz( graspos(icp(isys,1,i),1),
     +                graspos(icp(isys,1,i),2),
     +                graspos(icp(isys,1,i),3),
     +                xglen(i),
     +                yglen(i),
     +                zglen(i),
     +                graspos(icp(isys,3,i),1),
     +                graspos(icp(isys,3,i),2),
     +                graspos(icp(isys,3,i),3),
     +                distx,distz)
         print *,'new distx distz ',distx,distz
C--
         xcom=(  abs(graspos(icp(isys,2,1),1)) +
     +           abs(graspos(icp(isys,2,3),1)) )/2.


 
c         print *,'sag ind ',sag(i)

C proj to zero
c      rfits(10)=0.
c      rfits(13)=0.
c      rfits(14)=0.
c      rfits(15)=0.
c      rfits(16)=0.

C     INPLANE TO ZERO
          do k=1,50
             rfits(k)=rsave(k)
          enddo
      rfits(11)=0.
      rfits(12)=0.
      rfits(17)=0.
      rfits(18)=0.
      call shift(xgnom(i),ygnom(i),zgnom(i),
     +     xbml,ybml,zbml)
      a=xbml-xgnom(i)
      b=ybml-ygnom(i)
      c=zbml-zgnom(i)
      do k=1,50
         rfits(k)=rsave(k)
      enddo
      call shift(xgnom(i),ygnom(i),zgnom(i),
     +     xbml,ybml,zbml)

c          call shift(xgnom(i),ygnom(i),zgnom(i),
c     +               xbml,ybml,zbml)
c          print *,'diff x ',a,xbml-xgnom(i),a-(xbml-xgnom(i))
c          print *,'diff y ',b,ybml-ygnom(i),b-(ybml-ygnom(i))
c          print *,'diff z ',c,zbml-zgnom(i),c-(zbml-zgnom(i))

c          xglen(i)=xglen(i)-(xbml-xgnom(i))+a
c          yglen(i)=yglen(i)-(ybml-ygnom(i))+b
c          zglen(i)=zglen(i)-(zbml-zgnom(i))+c

         exaa(1)=xglen(i)-graspos(icp(isys,2,i),1)
         exaa(2)=yglen(i)-graspos(icp(isys,2,i),2)
         exaa(3)=zglen(i)-graspos(icp(isys,2,i),3)
        sag(i)=         0.*exaa(1)+
     +        cos(theta)*exaa(2)+
     +        -sin(theta)*exaa(3)


      enddo

C-- test
      do i=1,4
            xx2= graspos(icp(isys,2,i),1)
            yy2= graspos(icp(isys,2,i),2)
            zz2= graspos(icp(isys,2,i),3)
            yy1=ycham(1)
            yy3=ycham(3)
            xx1=xx2/ycham(2) * ycham(1)
            xx3=xx1+(xx2-xx1)/(ycham(2)-ycham(1))*
     +             (ycham(3)-ycham(1))
            zz1=zz2/ycham(2) * ycham(1)
            zz3=zz1+(zz2-zz1)/(ycham(2)-ycham(1))*
     +              (ycham(3)-ycham(1))

C--
C--   
            do k=1,50
               rfits(k)=rsave(k)
            enddo
            call cgeom(-1)
            call initras(-1)
            call predict(isys)
            call shift(xx1,yy1,zz1,xbil,ybil,zbil)
            call shift(xx2,yy2,zz2,xbml,ybml,zbml)
            call shift(xx3,yy3,zz3,xbol,ybol,zbol)
            call distxz( xbil,ybil,zbil,
     +           xbml,ybml,zbml,
     +           xbol,ybol,zbol,
     +                distx,distz)
            print *,'old test distx distz ',distx,distz
            
            rfits(10)=0.
            rfits(13)=0.
            rfits(14)=0.
            rfits(15)=0.
            rfits(16)=0.
            call shift(xx1,yy1,zz1,xbil,ybil,zbil)
            call shift(xx2,yy2,zz2,xbml,ybml,zbml)
            call shift(xx3,yy3,zz3,xbol,ybol,zbol)
         call distxz( xbil,ybil,zbil,
     +           xbml+xglen(i)-xgnom(i),
     +           ybml+yglen(i)-ygnom(i),
     +           zbml+zglen(i)-zgnom(i),
     +           xbol,ybol,zbol,
     +           distx,distz)
         print *,'new test distx distz ',distx,distz
C--
      enddo
C-- end test

          do k=1,50
             rfits(k)=rsave(k)
          enddo
      isys=4
      call cgeom(-1)
      call initras(-1)
      call predict(isys)


      print *,'mysag direct ',(sag(1)+sag(2)+sag(3)+sag(4))/4.

         open(20,file='lenspos.dat')
         do i=1,4
            write(20,*)xgnom(i)
            write(20,*)ygnom(i)
            write(20,*)zgnom(i)
            write(20,*)xglen(i)
            write(20,*)yglen(i)
            write(20,*)zglen(i)
            dy=graspos(icp(4,3,i),2)-graspos(icp(4,1,i),2)
            dz=graspos(icp(4,3,i),3)-graspos(icp(4,1,i),3)
            theta=atan2(dy,dz)
            write(20,*)theta
         enddo
C optionally, linked to above
c         rsave(10)=0.
c         rsave(13)=0.
c         rsave(14)=0.
c         rsave(15)=0.
c         rsave(16)=0.
         do i=1,50
            write(20,*)rsave(i)
         enddo
         close(20)

      end











      subroutine distxz(x1,y1,z1,x2,y2,z2,x3,y3,z3,
     +      distx,distz)
      double precision x1,y1,z1,x2,y2,z2,x3,y3,z3,
     +      distx,distz
    
C get distz      
      dzdy=(z3-z1)/(y3-y1)
      b=z1-dzdy*y1
      a=dzdy
C     closest approach z= a y + b
      if (a.ne.0.d0) then
         ap=-1.d0/a
      else
         ap=999999.d0
         print *,'WARININ getsag a=0 '
      endif
      bp=z2-ap*y2
      if (a-ap.ne.0.d0) then
         yl=(bp-b)/(a-ap)
      else
         yl=999999.d0
         print *,'WARININ getsag a-ap=0 '
      endif
      zl=a*yl+b
      if (abs(zl - (ap*yl+bp)).gt.0.1) 
     +     print *,'SHIT getsag ',i,zl,(ap*yl+bp)
      d=sqrt( (yl-y2)**2 + (zl-z2)**2 )
      if (zl.lt.z2) d=-d
      distz=d
C get distx      
      dxdy=(x3-x1)/(y3-y1)
      b=x1-dxdy*y1
      a=dxdy
C     closest approach z= a y + b
      if (a.ne.0.d0) then
         ap=-1.d0/a
      else
         ap=999999.d0
         print *,'WARININ getsag a=0 '
      endif
      bp=x2-ap*y2
      if (a-ap.ne.0.d0) then
         yl=(bp-b)/(a-ap)
      else
         yl=999999.d0
         print *,'WARININ getsag a-ap=0 '
      endif
      xl=a*yl+b
      if (abs(xl - (ap*yl+bp)).gt.0.1) 
     +     print *,'SHIT getsag ',i,zl,(ap*yl+bp)
      d=sqrt( (yl-y2)**2 + (xl-x2)**2 )
      if (zl.lt.x2) d=-d
      distx=d

      end

      subroutine sagcal(posbil,posbml,posbol,sag)
      implicit none
      include 'mcommon'
      double precision posbil(3),posbml(3),posbol(3),
     + xgnom(4),ygnom(4),zgnom(4),
     + xglen(4),yglen(4),zglen(4),
     + xbil,ybil,zbil,xbml,ybml,zbml,xbol,ybol,zbol,
     + xcor,ycor,zcor,
     + daa,dbb,a,b,c,d,dzdy,ap,bp,yl,zl,
     + tsag(4),theta,exaa(3),dz,dy,ax,ay,az,tfits(50),rsave(50),
     + dsag,dxav,dyav,dzav,thav,dysag,dzsag,hoek(4)
      double precision xx1,xx2,xx3,yy1,yy2,yy3,zz1,zz2,zz3
      real sag
      integer i
      logical first
      common /sjsjdjfd/first,xgnom,ygnom,zgnom,xglen,yglen,zglen,
     +                tfits,hoek
      data first /.TRUE./


      if (first)  then
         open(20,file='lenspos.dat')
         do i=1,4
            read(20,*)xgnom(i)
            read(20,*)ygnom(i)
            read(20,*)zgnom(i)
            read(20,*)xglen(i)
            read(20,*)yglen(i)
            read(20,*)zglen(i)
            read(20,*)hoek(i)
         enddo
         do i=1,50
            read(20,*)rfits(i)
         enddo
         close(20)
      endif

      xbil=posbil(1)
      ybil=posbil(2)
      zbil=posbil(3)
      xbml=posbml(1)
      ybml=posbml(2)
      zbml=posbml(3)
      xbol=posbol(1)
      ybol=posbol(2)
      zbol=posbol(3)

      call shift(posbil(1),posbil(2),posbil(3),
     +             xbil,ybil,zbil)
      call shift(posbml(1),posbml(2),posbml(3),
     +             xbml,ybml,zbml)
      call shift(posbol(1),posbol(2),posbol(3),
     +             xbol,ybol,zbol)

      daa=(xbml-xglen(1))/(xglen(3)-xglen(1))
      dbb=(zbml-zglen(1))/(zglen(2)-zglen(1))
      a=xglen(1)-xgnom(1)
      c=xglen(2)-xgnom(2)
      b=xglen(3)-xgnom(3)
      d=xglen(4)-xgnom(4)
      call fpint(xcor,daa,dbb,a,b,c,d)
      xcor=xcor+xbml
      a=zglen(1)-zgnom(1)
      c=zglen(2)-zgnom(2)
      b=zglen(3)-zgnom(3)
      d=zglen(4)-zgnom(4)
      call fpint(zcor,daa,dbb,a,b,c,d)
c      print *,' abcd zcor',a,b,c,d,zcor
      zcor=zcor+zbml
      dz=(a+b+c+d)/4.
      ay=((a+c)/2.-(b+d)/2.)/(xgnom(1)-xgnom(3))
      a=yglen(1)-ygnom(1)
      c=yglen(2)-ygnom(2)
      b=yglen(3)-ygnom(3)
      d=yglen(4)-ygnom(4)
      call fpint(ycor,daa,dbb,a,b,c,d)
      ycor=ycor+ybml

C actual sagitta calculation
      dzdy=(zbol-zbil)/(ybol-ybil)
      b=zbil-dzdy*ybil
      a=dzdy
      
C     closest approach z= a y + b
      if (a.ne.0.d0) then
         ap=-1.d0/a
      else
         ap=999999.d0
         print *,'WARININ getsag a=0 '
      endif
      bp=zcor-ap*ycor
      if (a-ap.ne.0.d0) then
         yl=(bp-b)/(a-ap)
      else
         yl=999999.d0
         print *,'WARININ getsag a-ap=0 '
      endif
      zl=a*yl+b
      if (abs(zl - (ap*yl+bp)).gt.0.1) 
     +     print *,'SHIT getsag ',i,zl,(ap*yl+bp)
      d=sqrt( (yl-ycor)**2 + (zl-zcor)**2 )
      if (zl.lt.zcor) d=-d

      sag=d

      first=.FALSE.
      end
      




      subroutine sagclas(posbil,posbml,posbol,sag)
      implicit none
      include 'mcommon'
      double precision posbil(3),posbml(3),posbol(3),
     + xgnom(4),ygnom(4),zgnom(4),
     + xglen(4),yglen(4),zglen(4),
     + xbil,ybil,zbil,xbml,ybml,zbml,xbol,ybol,zbol,
     + xcor,ycor,zcor,
     + daa,dbb,a,b,c,d,dzdy,ap,bp,yl,zl,
     + tsag(4),theta,exaa(3),dz,dy,ax,ay,az,tfits(50),rsave(50),
     + dsag,dxav,dyav,dzav,thav,dysag,dzsag,hoek(4)
      double precision xx1,xx2,xx3,yy1,yy2,yy3,zz1,zz2,zz3
      real sag
      integer i
      logical first
      common /sjsjdjfd/first,xgnom,ygnom,zgnom,xglen,yglen,zglen,
     +                tfits,hoek
      data first /.TRUE./


      if (first)  then
         open(20,file='lenspos.dat')
         do i=1,4
            read(20,*)xgnom(i)
            read(20,*)ygnom(i)
            read(20,*)zgnom(i)
            read(20,*)xglen(i)
            read(20,*)yglen(i)
            read(20,*)zglen(i)
            read(20,*)hoek(i)
         enddo
         do i=1,50
            read(20,*)rfits(i)
         enddo
         close(20)
      endif
C rfits(i) has zero values projective parameters for BML,
C because these are directly calculated.

C-------
c            xx2=xgnom(3)
c            zz2=zgnom(3)

c            yy1=ycham(1)
c            yy2=ycham(2)
c            yy3=ycham(3)
c            xx1=xx2/ycham(2) * ycham(1)
c            xx3=xx1+(xx2-xx1)/(ycham(2)-ycham(1))*
c     +             (ycham(3)-ycham(1))
c            zz1=zz2/ycham(2) * ycham(1)
c            zz3=zz1+(zz2-zz1)/(ycham(2)-ycham(1))*
c     +              (ycham(3)-ycham(1))
c            posbil(1)=xx1
c            posbil(2)=yy1
c            posbil(3)=zz1
c            posbml(1)=xx2
c            posbml(2)=yy2
c            posbml(3)=zz2
c            posbol(1)=xx3
c            posbol(2)=yy3
c            posbol(3)=zz3
C-----------


      xbil=posbil(1)
      ybil=posbil(2)
      zbil=posbil(3)
      xbml=posbml(1)
      ybml=posbml(2)
      zbml=posbml(3)
      xbol=posbol(1)
      ybol=posbol(2)
      zbol=posbol(3)

      call shift(posbil(1),posbil(2),posbil(3),
     +             xbil,ybil,zbil)
      call shift(posbml(1),posbml(2),posbml(3),
     +             xbml,ybml,zbml)
      call shift(posbol(1),posbol(2),posbol(3),
     +             xbol,ybol,zbol)

      

c      daa=(xbml-xgnom(2))/(xgnom(4)-xgnom(2))
c      dbb=(zbml-zgnom(1))/(zgnom(2)-zgnom(1))
c      print *,'old daa dbb ',daa,dbb
      daa=(xbml-xgnom(1))/(xgnom(3)-xgnom(1))
      dbb=(zbml-zgnom(1))/(zgnom(2)-zgnom(1))
c      print *,'new daa dbb ',daa,dbb

      a=xglen(1)-xgnom(1)
      c=xglen(2)-xgnom(2)
      b=xglen(3)-xgnom(3)
      d=xglen(4)-xgnom(4)
      call fpint(xcor,daa,dbb,a,b,c,d)
      xcor=xcor+xbml


c      print *,'dx 1 ',a
c      print *,'dx 2 ',b
c      print *,'dx 3 ',c
c      print *,'dx 4 ',d


      a=zglen(1)-zgnom(1)
      c=zglen(2)-zgnom(2)
      b=zglen(3)-zgnom(3)
      d=zglen(4)-zgnom(4)
      call fpint(zcor,daa,dbb,a,b,c,d)
      print *,' abcd zcor',a,b,c,d,zcor
      zcor=zcor+zbml


c      print *,'dz 1 ',a
c      print *,'dz 2 ',b
c      print *,'dz 3 ',c
c      print *,'dz 4 ',d


      dz=(a+b+c+d)/4.


      ay=((a+c)/2.-(b+d)/2.)/(xgnom(1)-xgnom(3))

      a=yglen(1)-ygnom(1)
      c=yglen(2)-ygnom(2)
      b=yglen(3)-ygnom(3)
      d=yglen(4)-ygnom(4)
      call fpint(ycor,daa,dbb,a,b,c,d)
      ycor=ycor+ybml

C TEST remove
c      print *,' y z voor ',ycor,zcor
c      zcor=zglen(4)-zgnom(4)+zbml
c      ycor=yglen(4)-ygnom(4)+ybml
c      print *,' y z na   ',ycor,zcor

c      print *,'ycor ',ycor,ybml,dy,daa,dbb
c      print *,'test dy ',(a+b+c+d)/4.
c      print *,'az ',((a+c)/2.-(b+d)/2.)/(xgnom(1)-xgnom(3))
c      print *,'ax ',((a+b)/2.-(c+d)/2.)/(zgnom(1)-zgnom(2))
c      print *,'bml ',xbml,ybml,zbml
c      print *,'cor ',xcor,ycor,zcor

      dy=(a+b+c+d)/4.
      az=((a+c)/2.-(b+d)/2.)/(xgnom(1)-xgnom(3))
c      print *,'dy 1 ',a
c      print *,'dy 2 ',b
c      print *,'dy 3 ',c
c      print *,'dy 4 ',d

      ax=((a+b)/2.-(c+d)/2.)/(zgnom(1)-zgnom(2))

C the following code allows to go back to a parameter representation
C of the projective correction, obtained above. For tests.
      if (first) then
      do i=1,50
         tfits(i)=rfits(i)
      enddo
      tfits(10)=rfits(10)+ax
      tfits(13)=rfits(13)+ay
      tfits(14)=rfits(14)+az
      tfits(15)=rfits(15)+dy
      tfits(16)=rfits(16)+dz
      open (45,file='direct.dat')
      do i=1,50
         write(45,*)tfits(i)
      enddo
      close(45)
      endif
C------end

c test
      dxav=0.
      dyav=0.
      dzav=0.
      thav=0.
      dysag=0.
      dzsag=0.
      do i=1,4
c         dy=graspos(icp(4,3,i),2)-graspos(icp(4,1,i),2)
c         dz=graspos(icp(4,3,i),3)-graspos(icp(4,1,i),3)
c         dy=ybol-ybil
c         dz=zbol-zbil
c         theta=atan2(dy,dz)
         theta=hoek(i)
         exaa(1)=xglen(i)-xgnom(i)
         exaa(2)=yglen(i)-ygnom(i)
         exaa(3)=zglen(i)-zgnom(i)
         dxav=dxav+exaa(1)/4.
         dyav=dyav+exaa(2)/4.
         dzav=dzav+exaa(3)/4.
         thav=thav+theta/4.
        tsag(i)=         0.*exaa(1)+
     +        cos(theta)*exaa(2)+
     +        -sin(theta)*exaa(3)
        dysag= dysag+0.25*cos(theta)
        dzsag= dzsag-0.25*sin(theta)
        enddo
c        print *,'sagcal ttttest ',    
c     +          (tsag(1)+tsag(2)+tsag(3)+tsag(4))/4.
       call fpint(dsag,daa,dbb,tsag(1),tsag(3),tsag(2),tsag(4))
C TEST remove
c       dsag=tsag(4)
c       zcor=zbml
c       ycor=ybml
c        dsag=0.
c end tes


c      do i=1,50
c         rsave(i)=rfits(i)
c         rfits(i)=tfits(i)
c      enddo
c      rfits(11)=0.
c      rfits(12)=0.
c      rfits(17)=0.
c      rfits(18)=0.
c      call shift(posbml(1),posbml(2),posbml(3),
c     +             xbml,ybml,zbml)
c      print *,'--------------------------'
c      print *,'xbml test ',xbml,xcor
c      print *,'zbml test ',zbml,zcor
c      print *,'ybml test ',ybml,ycor
c      zcor=zbml
c      ycor=ybml
c
c      do i=1,50
c         rfits(i)=rsave(i)
c      enddo


C actual sagitta calculation
      dzdy=(zbol-zbil)/(ybol-ybil)
      b=zbil-dzdy*ybil
      a=dzdy
      
C     closest approach z= a y + b
      if (a.ne.0.d0) then
         ap=-1.d0/a
      else
         ap=999999.d0
         print *,'WARININ getsag a=0 '
      endif
      bp=zcor-ap*ycor
      if (a-ap.ne.0.d0) then
         yl=(bp-b)/(a-ap)
      else
         yl=999999.d0
         print *,'WARININ getsag a-ap=0 '
      endif
      zl=a*yl+b
      if (abs(zl - (ap*yl+bp)).gt.0.1) 
     +     print *,'SHIT getsag ',i,zl,(ap*yl+bp)
      d=sqrt( (yl-ycor)**2 + (zl-zcor)**2 )
      if (zl.lt.zcor) d=-d

      sag=d

c      print *,' ttttttest sagcal sag imp ',sag

C-----------test
      zcor=zbml
      ycor=ybml

C actual sagitta calculation
      dzdy=(zbol-zbil)/(ybol-ybil)
      b=zbil-dzdy*ybil
      a=dzdy
      
C     closest approach z= a y + b
      if (a.ne.0.d0) then
         ap=-1.d0/a
      else
         ap=999999.d0
         print *,'WARININ getsag a=0 '
      endif
      bp=zcor-ap*ycor
      if (a-ap.ne.0.d0) then
         yl=(bp-b)/(a-ap)
      else
         yl=999999.d0
         print *,'WARININ getsag a-ap=0 '
      endif
      zl=a*yl+b
      if (abs(zl - (ap*yl+bp)).gt.0.1) 
     +     print *,'SHIT getsag ',i,zl,(ap*yl+bp)
      d=sqrt( (yl-ycor)**2 + (zl-zcor)**2 )
      if (zl.lt.zcor) d=-d


      sag=real(d+dsag)

      first=.FALSE.
      end


*-- Author :    Marcel Vreeswijk   03/03/94
      SUBROUTINE FPINT(fval,dxx,dyy,a,b,c,d)
******************************************************************
*                                                                *
* FPINT (version 1.00)                                           *
*                                                                *
*                                                                *
* IN:
* interpolate linear on a four point grid                        *
* dyy= dy/ybinwid
* dxx= dx/xbinwid
*            c ------- d
*            |         |
*            a ------- b   function values                       *
* OUT:   fval corresponding to dxx dyy                           *
*                                                                *
*                                                                *
*  written by: Marcel Vreeswijk, 03/03/94 10.53.52               *
* modified by:                                                   *
******************************************************************
      double precision Fval,dxx,dyy,a,b,c,d
      Fval  = a+ dxx*(b-a)+ dyy*(
     +     (c+ dxx * (d -c) )-
     +     (a+ dxx * (b -a)) )
      end


      
      subroutine seticp(ipos,line)
C====================================================
C sets icp to track RASNIK component position
C
C icp(
C     1=BIL 2=BML 3=BOL, 4=PRO
C     1=MSK 2=LEN 3=CCD
C     1=0=Q0 2=1=P0 3=2=Q1 4=3=P1)= array position
C====================================================
      implicit none
      include 'mcommon'
      character*80 line
      integer ipos
      integer i,isys,ityp,j
      isys=-1
      ityp=-1
      j=-1
      do i=5,19 ! or so
ccc         if (line(i:i+2).eq.'INP') 
         if (line(i:i+2).eq.'PRO') isys=4
         if (isys.ne.4) then  ! PRO has priority
            if (line(i:i+2).eq.'BIL') isys=1
            if (line(i:i+2).eq.'BML') isys=2
            if (line(i:i+2).eq.'BOL') isys=3
         endif
         if (line(i:i+2).eq.'MSK') ityp=1
         if (line(i:i+2).eq.'LEN') ityp=2
         if (line(i:i+2).eq.'CCD') ityp=3         
      enddo
      j=-1
      do i=15,21
         if (line(i:i).eq.'0') j=1
         if (line(i:i).eq.'1') j=2
         if (line(i:i).eq.'2') j=3
         if (line(i:i).eq.'3') j=4
      enddo
      if (isys.lt.0
     +              .or. ityp.lt.0 .or. j.lt.0) then
         print *,'FATAL seticp ',ipos,line,isys,ityp,j
         stop
      endif
      icp(isys,ityp,j)=ipos
C Expand the CCD system to 4 of the inplane chambers CCD0=CC2 CCD1=CCD3
      if (isys.le.3 .and. ityp.eq.3) then
         icp(isys,ityp,j+2)=ipos
      endif
      end

      subroutine initras(ifile)
C=============================================================
C read from object files: obj.'ifile'. 
C Usually ifile=-1 in fitting mode.
C=============================================================
      implicit none
      include 'mcommon'
      integer iobj,i,j,k,idum,ifile
      character*80 line
      character*20 name
      character*4 subname
      double precision a,b,c
      integer kcp,ityp
      common /ssdgsd/first,rasposorg(100,3)
      double precision rasposorg
      double precision t1,t2,t3,t4
      double precision xgmsk,xglen,xgccd,
     +                 ygmsk,yglen,ygccd,
     +                 zgmsk,zglen,zgccd,
     +                 xglen1,yglen1,zglen1,dm,delta
      integer km,kl,kc
      logical first
      data first /.TRUE./


      if (ifile.ge.0) then
C     read object file
         write(subname,'(I4)')ifile
         name='../lini/obj.'//subname
         open(33,file=name)
         read(33,*)iobj
         do i=1,iobj
            read(33,'(a)')line
            read(33,*)a,b,c
         DO J=1,3
            read(33,*)cp(i,1,j),cp(i,2,j),cp(i,3,j),cppos(i,j)
c            read(33,*)t1,t2,t3,t4
c            print *,i,j,'--------------------------------'
c            print *,t1-cp(i,1,j)
c            print *,t2-cp(i,2,j)
c            print *,t3-cp(i,3,j)
c            print *,t4-cppos(i,j)
c            cp(i,1,j)=t1
c            cp(i,2,j)=t2
c            cp(i,3,j)=t3
c            cppos(i,j)=t4
         ENDDO
      enddo
      close(33)
      endif

      if (first) then
C     read component file
         open(33,file='datcha_1.cmp')
         read(33,*)iobj
         do i=1,iobj
            read(33,'(a)')line
            open(34,file='stom')
            write(34,*)line(1:4)
            close(34)
            open(34,file='stom')
            read(34,*)idum,rastocp(i)
            call seticp(idum+1,line)
            rastocp(i)=rastocp(i)+1 ! one offset compensates for 0....iobj-1
            close(34)
            DO J=1,3
               read(33,*)ras(i,1,j),ras(i,2,j),ras(i,3,j),rasposorg(i,j)
               raspos(i,j)=rasposorg(i,j)
            ENDDO
      enddo
      close(33)
      endif

C get positions components----------------------------------------
C projective
C 0=Q0       2=Q1
C 1=P0       3=P3
C  Vcp = ras Vras
C  Vglobal = cp Vcp

c      do isys=1,4
c         do ityp=1,3
c            do j=1,4
c               k=icp(isys,ityp,j)
c               raspos(i,j)=rasposorg(i,j)*(1.+

      do k=1,42
         do i=1,3
            rasposp(k,i)=0.
            do j=1,3
               kcp=rastocp(k)
               rasposp(k,i)=rasposp(k,i)+cp(kcp,j,i)*raspos(k,j)
            enddo
         enddo
      enddo

      do k=1,42
      do i=1,3               
         kcp=rastocp(k)
         graspos(k,i)=cppos(kcp,i)+rasposp(k,i)
C not k dependent, but anyway here
         bilpos(1,i)=cppos(1,i)+rasposp(31,i) 
         bilpos(2,i)=cppos(1,i)+rasposp(32,i) 
         bilpos(3,i)=cppos(3,i)+rasposp(33,i) 
         bilpos(4,i)=cppos(3,i)+rasposp(34,i)
         
         bmlpos(1,i)=cppos(4,i)+rasposp(35,i) 
         bmlpos(2,i)=cppos(4,i)+rasposp(36,i) 
         bmlpos(3,i)=cppos(6,i)+rasposp(37,i) 
         bmlpos(4,i)=cppos(6,i)+rasposp(38,i)
         
         bolpos(1,i)=cppos(7,i)+rasposp(39,i) 
         bolpos(2,i)=cppos(7,i)+rasposp(40,i) 
         bolpos(3,i)=cppos(9,i)+rasposp(41,i) 
         bolpos(4,i)=cppos(9,i)+rasposp(42,i)
      enddo
      enddo
c     km=icp(4,1,1)      
c     kl=icp(4,2,1)
c     kc=icp(4,3,1)
c     delta=0.02/(1+1.3)
c     delta=0.02
c     dm=1.28
c     xgmsk=graspos(km,1)
c     xgccd=graspos(kc,1)
c     ygmsk=graspos(km,2)
c     ygccd=graspos(kc,2)
c     zgmsk=graspos(km,3)
c     zgccd=graspos(kc,3)
c     xglen=xgmsk+ (xgccd-xgmsk)
c    +     /(1.+dm)
c     yglen=ygmsk+ (ygccd-ygmsk)
c    +     /(1.+dm)
c     zglen=zgmsk+ (zgccd-zgmsk)
c    +     /(1.+dm)

c     xglen1=xgmsk+ (xgccd-xgmsk)
c    +     /(1.+dm+delta)
c     yglen1=ygmsk+ (ygccd-ygmsk)
c    +     /(1.+dm+delta)
c     zglen1=zgmsk+ (zgccd-zgmsk)
c    +     /(1.+dm+delta)
      
c     print *,'bef ',graspos(kl,1),graspos(kl,2),graspos(kl,3)
c     graspos(kl,1)=graspos(kl,1)+xglen1-xglen
c     graspos(kl,2)=graspos(kl,2)+yglen1-yglen
c     graspos(kl,3)=graspos(kl,3)+zglen1-zglen
c     print *,'na ',graspos(kl,1),graspos(kl,2),graspos(kl,3)
      end


      subroutine predict(isys)
C=============================================================
C predicts RASNIK values based on object file read by initras.
C routine snijpunt is used to get crossing of line (BML BOL)
C to mask surface (BIL).
C Rotation is calculated by projecting a second point.
C=============================================================      
      implicit none
      include 'mcommon'
      double precision v,b
      double precision x0,y0,z0,aa(3),dd(3)
      double precision ex(3),exa(3),exaa(3),xp,yp
      double precision offx,offy,offz
      integer j,i1,i2,kcp,isys,k1,k2,k3
            
      do j=1,4
C--
C choose BOL RASNIKS as base for line BML BOL
C--
C--
C offset numbering
C 1: Cy
C 2: Cx
C 3: Cmag
C 4: theta_a
C 5: theta_s

Cadd
         call toglobccd(isys,j,offset(isys,j,1),
     +                    offset(isys,j,2),
     +                    0.d0,
     +                    offx,offy,offz)
         x0=graspos(icp(isys,3,j), 1)  + offx
         y0=graspos(icp(isys,3,j), 2)  + offy
         z0=graspos(icp(isys,3,j), 3)  + offz
Cendadd
         call crosspoint(isys,j,x0,y0,z0,dd(1),dd(2),dd(3))
         xras(j)=dd(1)
         yras(j)=dd(2)
         zras(j)=dd(3)
C--
C get rotation: start with point away from center by unit in x
C             : then goto global frame
C--
C     unit in x
         ex(1)=1.
         ex(2)=0.
         ex(3)=0.
Cadd
         call torotras(offset(isys,j,4),ex(1),ex(2),xp,yp)
         ex(1)=xp
         ex(2)=yp
Cendadd

C     goto component frame
         do i1=1,3
            exa(i1)=0.
            do i2=1,3
               exa(i1)=exa(i1)+ras(icp(isys,3,j),i2,i1)*ex(i2)
            enddo
         enddo
C     goto global frame
         do i1=1,3
            exaa(i1)=0.
            do i2=1,3
               kcp=rastocp(icp(isys,3,j))
               exaa(i1)=exaa(i1)+cp(kcp,i2,i1)*exa(i2)
            enddo
         enddo
Cadd
         x0=graspos(icp(isys,3,j), 1)+offx+exaa(1)
         y0=graspos(icp(isys,3,j), 2)+offy+exaa(2)
         z0=graspos(icp(isys,3,j), 3)+offz+exaa(3)
Cendadd
         call crosspoint(isys,j,x0,y0,z0,aa(1),aa(2),aa(3))
         rotras(j)=atan2(0. , -1.) - atan2( aa(2)-dd(2) , aa(1)-dd(1) )
         if (rotras(j).gt.atan2(0., -1.)) then
            rotras(j)=rotras(j)-2.*atan2(0.,-1.)
         endif
         
C     magnification
         k1=icp(isys,1,j)
         k2=icp(isys,2,j)
         k3=icp(isys,3,j)
         v=sqrt(  (graspos(k1,1)-graspos(k2,1))**2+
     +        (graspos(k1,2)-graspos(k2,2))**2+
     +        (graspos(k1,3)-graspos(k2,3))**2 )
         b=sqrt(  (graspos(k2,1)-graspos(k3,1))**2+
     +        (graspos(k2,2)-graspos(k3,2))**2+
     +        (graspos(k2,3)-graspos(k3,3))**2 )
Cadd
         dmras(j)=b/v+offset(isys,jco,3)
Cendadd
      enddo
      end	

	subroutine toloc(isys,j,x,y,z,xuse,yuse,zuse)
        implicit none
      include 'mcommon'
        double precision ex(3),exa(3),exaa(3)
        double precision x,y,z,xuse,yuse,zuse
        integer kcp,isys,i1,i2,i,j,IR(9),ifail,jj
	double precision cpi(3,3),rasi(3,3)

C     get inverse ras
      do i1=1,3
         do i2=1,3
            rasi(i2,i1)=ras(icp(isys,3,j),i2,i1)
         enddo
      enddo

      call DINV(3,rasi,3,IR,ifail)
      if (ifail.ne.0) print *,' error rasi toloc '

C     get inverse cp
      do i1=1,3
         do i2=1,3
            kcp=rastocp(icp(isys,3,j))
            cpi(i2,i1)=cp(kcp,i2,i1)
         enddo
      enddo
      call DINV(3,cpi,3,IR,ifail)
      if (ifail.ne.0) print *,' error cpi toloc '
C     unit in x
         ex(1)=x
         ex(2)=y
         ex(3)=z
C     goto component frame
         do i1=1,3
            exa(i1)=0.
            do i2=1,3
               exa(i1)=exa(i1)+cpi(i2,i1)*ex(i2)
            enddo
         enddo
C     goto global frame
         do i1=1,3
            exaa(i1)=0.
            do i2=1,3
               exaa(i1)=exaa(i1)+rasi(i2,i1)*exa(i2)
            enddo
         enddo
         xuse=exaa(1)
         yuse=exaa(2)
         zuse=exaa(3)
         end

	
	subroutine toglobmsk(isys,j,x,y,z,xuse,yuse,zuse)
	implicit none
      include 'mcommon'
	double precision ex(3),exa(3),exaa(3)
	double precision x,y,z,xuse,yuse,zuse,xp,yp
	integer kcp,i1,i2,i,j,isys
C     unit in x
         ex(1)=x
         ex(2)=y
         ex(3)=z

Cadd
         call torotras(offset(isys,j,5),ex(1),ex(2),xp,yp)
         ex(1)=xp
         ex(2)=yp
Cendadd


C     goto component frame
         do i1=1,3
            exa(i1)=0.
            do i2=1,3
               exa(i1)=exa(i1)+ras(icp(isys,1,j),i2,i1)*ex(i2)
            enddo
         enddo
C     goto global frame
         do i1=1,3
            exaa(i1)=0.
            do i2=1,3
               kcp=rastocp(icp(isys,1,j))
               exaa(i1)=exaa(i1)+cp(kcp,i2,i1)*exa(i2)
            enddo
         enddo

	 xuse=exaa(1)
	 yuse=exaa(2)
	 zuse=exaa(3)
	 end


	subroutine toglobccd(isys,j,x,y,z,xuse,yuse,zuse)
	implicit none
      include 'mcommon'
	double precision ex(3),exa(3),exaa(3)
	double precision x,y,z,xuse,yuse,zuse,xp,yp
	integer kcp,i1,i2,i,j,isys
C     unit in x
         ex(1)=x
         ex(2)=y
         ex(3)=z
Cadd
         call torotras(offset(isys,j,4),ex(1),ex(2),xp,yp)
         ex(1)=xp
         ex(2)=yp
Cendadd

C     goto component frame
         do i1=1,3
            exa(i1)=0.
            do i2=1,3
               exa(i1)=exa(i1)+ras(icp(isys,3,j),i2,i1)*ex(i2)
            enddo
         enddo
C     goto global frame
         do i1=1,3
            exaa(i1)=0.
            do i2=1,3
               kcp=rastocp(icp(isys,3,j))
               exaa(i1)=exaa(i1)+cp(kcp,i2,i1)*exa(i2)
            enddo
         enddo

	 xuse=exaa(1)
	 yuse=exaa(2)
	 zuse=exaa(3)
	 end



      subroutine crosspoint(isys,j,x0,y0,z0,xss,yss,zss)
C=========================================================
C returns  crossing point line BMLcenter BOL_x0,y0,z0 with
C mask on BIL in xs,ys,zs
C=========================================================
      implicit none
      include 'mcommon'
      DOUBLE PRECISION ddx,ddy,ddz,xss,yss,zss
      double precision x0,y0,z0,ex(3),ey(3),exa(3),eya(3),
     +     exaa(3),eyaa(3),xm,ym,zm,
     +     ddxk,ddyk,ddzk,ddxq,ddyq,ddzq,
     +     dd(3),ddold(3),V(3,3),
     +     cpi(3,3),rasi(3,3),xp,yp
      integer i,j,i1,i2,ifail,kcp,ir(9),isys
     
C     line BOL BML Y X to get dxras. Thus (x,y,z)=(x0,y0,z0)+ k (ddx,ddy,ddz)
      ddx=graspos(icp(isys,2,j),1)-x0
      ddy=graspos(icp(isys,2,j),2)-y0
      ddz=graspos(icp(isys,2,j),3)-z0
      
C parametrise mask on BIL:
C     (x,y,z)=(xm,ym,zm)+ k (ddxk,ddyk,ddzk) + q ((ddxq,ddyq,ddzq)
C     RASNIK frame
      ex(1)=1
      ex(2)=0
      ex(3)=0
      ey(1)=0
      ey(2)=1
      ey(3)=0
C     goto component frame
Cadd    symmetric angle
        call torotras(offset(isys,j,5),ex(1),ex(2),xp,yp)
        ex(1)=xp
        ex(2)=yp
        call torotras(offset(isys,j,5),ey(1),ey(2),xp,yp)
        ey(1)=xp
        ey(2)=yp
Cendadd

      do i1=1,3
         exa(i1)=0.
         eya(i1)=0.
         do i2=1,3
            exa(i1)=exa(i1)+ras(icp(isys,1,j),i2,i1)*ex(i2)
            eya(i1)=eya(i1)+ras(icp(isys,1,j),i2,i1)*ey(i2)
         enddo
      enddo


C     goto global frame
      do i1=1,3
         exaa(i1)=0.
         eyaa(i1)=0.
         do i2=1,3
            kcp=rastocp(icp(isys,1,j))
c     print *,'should be 1 1 3 3 ',kcp
            exaa(i1)=exaa(i1)+cp(kcp,i2,i1)*exa(i2)
            eyaa(i1)=eyaa(i1)+cp(kcp,i2,i1)*eya(i2)
         enddo
      enddo

C     finalise
      xm=graspos(icp(isys,1,j),1)
      ym=graspos(icp(isys,1,j),2)
      zm=graspos(icp(isys,1,j),3)
      ddxk=exaa(1)
      ddyk=exaa(2)
      ddzk=exaa(3)
      ddxq=eyaa(1)
      ddyq=eyaa(2)
      ddzq=eyaa(3)
      
C     l Vl + p (-Vk) + q (-Vq) = Vm-Vo
      V(1,1)=ddx
      V(2,1)=ddy
      V(3,1)=ddz
      V(1,2)=-ddxk
      V(2,2)=-ddyk
      V(3,2)=-ddzk
      V(1,3)=-ddxq
      V(2,3)=-ddyq
      V(3,3)=-ddzq
      
      dd(1)=xm-x0
      dd(2)=ym-y0
      dd(3)=zm-z0

      do i=1,3
         ddold(i)=dd(i)
      enddo
      call DEQN(3,V,3,IR,IFAIL,1,dd)
      if (ifail.ne.0) print *,'terug naar af ',ifail
      do i=1,3
         ddold(i)=dd(i)
      enddo
      
      dd(1)=x0+ddold(1)*ddx
      dd(2)=y0+ddold(1)*ddy
      dd(3)=z0+ddold(1)*ddz
      
C     get inverse ras
      do i1=1,3
         do i2=1,3
            rasi(i2,i1)=ras(icp(isys,1,j),i2,i1)
         enddo
      enddo
      
      call DINV(3,rasi,3,IR,ifail)
      if (ifail.ne.0) print *,' error rasi '
      
C     get inverse cp
      do i1=1,3
         do i2=1,3
            kcp=rastocp(icp(isys,1,j))
            cpi(i2,i1)=cp(kcp,i2,i1)
         enddo
      enddo
      call DINV(3,cpi,3,IR,ifail)
      if (ifail.ne.0) print *,' error cpi '
      
C     from global dd to CP dd
      ddold(1)=dd(1)-xm
      ddold(2)=dd(2)-ym
      ddold(3)=dd(3)-zm
      do i1=1,3
         dd(i1)=0.
         do i2=1,3
            dd(i1)=dd(i1)+cpi(i2,i1)*ddold(i2)
         enddo
      enddo


C     from CP to component
      do i1=1,3
         ddold(i1)=dd(i1)
      enddo
      do i1=1,3
         dd(i1)=0.
         do i2=1,3
            dd(i1)=dd(i1)+rasi(i2,i1)*ddold(i2)
         enddo
      enddo

Cadd
      call torotras(-offset(isys,j,5),dd(1),dd(2),xp,yp)
      dd(1)=xp
      dd(2)=yp
Cendadd

      xss=dd(1)
      yss=dd(2)
      zss=dd(3)
      end



      subroutine preras
C=============================================================
C predicts RASNIK values based on object file read by initras.
C routine snijpunt is used to get crossing of line (BML BOL)
C to mask surface (BIL).
C Rotation is calculated by projecting a second point.
C=============================================================      
      implicit none
      include 'mcommon'
      double precision v,b
      double precision x0,y0,z0,aa(3),dd(3)
      double precision ex(3),exa(3),exaa(3)
      integer j,i1,i2,kcp
            
      do j=1,4
C--
C choose BOL RASNIKS as base for line BML BOL
C--
         x0=bolpos(j,1)
         y0=bolpos(j,2)
         z0=bolpos(j,3)
         call snijpunt(j,x0,y0,z0,dd(1),dd(2),dd(3))
         xras(j)=dd(1)
         yras(j)=dd(2)
         zras(j)=dd(3)
C--
C get rotation: start with point away from center by unit in x
C             : then goto global frame
C--
C     unit in x
         ex(1)=1.
         ex(2)=0.
         ex(3)=0.
C     goto component frame
         do i1=1,3
            exa(i1)=0.
            do i2=1,3
               exa(i1)=exa(i1)+ras(38+j,i2,i1)*ex(i2)
            enddo
         enddo
C     goto global frame
         do i1=1,3
            exaa(i1)=0.
            do i2=1,3
               kcp=rastocp(38+j)
               exaa(i1)=exaa(i1)+cp(kcp,i2,i1)*exa(i2)
            enddo
         enddo
         x0=bolpos(j,1)+exaa(1)
         y0=bolpos(j,2)+exaa(2)
         z0=bolpos(j,3)+exaa(3)
         call snijpunt(j,x0,y0,z0,aa(1),aa(2),aa(3))
         rotras(j)=atan2(0. , -1.) - atan2( aa(2)-dd(2) , aa(1)-dd(1) )
         if (rotras(j).gt.atan2(0., -1.)) then
            rotras(j)=rotras(j)-2.*atan2(0.,-1.)
         endif
         
C     magnification
         v=sqrt(  (bilpos(j,1)-bmlpos(j,1))**2+
     +        (bilpos(j,2)-bmlpos(j,2))**2+
     +        (bilpos(j,3)-bmlpos(j,3))**2 )
         b=sqrt(  (bmlpos(j,1)-bolpos(j,1))**2+
     +        (bmlpos(j,2)-bolpos(j,2))**2+
     +        (bmlpos(j,3)-bolpos(j,3))**2 )
         dmras(j)=b/v
      enddo
      end	
      
      subroutine snijpunt(j,x0,y0,z0,xss,yss,zss)
C=========================================================
C returns  crossing point line BMLcenter BOL_x0,y0,z0 with
C mask on BIL in xs,ys,zs
C=========================================================
      implicit none
      include 'mcommon'
      DOUBLE PRECISION ddx,ddy,ddz,xss,yss,zss
      double precision x0,y0,z0,ex(3),ey(3),exa(3),eya(3),
     +     exaa(3),eyaa(3),xm,ym,zm,
     +     ddxk,ddyk,ddzk,ddxq,ddyq,ddzq,
     +     dd(3),ddold(3),V(3,3),
     +     cpi(3,3),rasi(3,3)
      integer i,j,i1,i2,ifail,kcp,ir(9)
     
C     line BIL BML Y X to get dxras. Thus (x,y,z)=(x0,y0,z0)+ k (ddx,ddy,ddz)
      ddx=bmlpos(j,1)-x0
      ddy=bmlpos(j,2)-y0
      ddz=bmlpos(j,3)-z0
      
C parametrise mask on BIL:
C     (x,y,z)=(xm,ym,zm)+ k (ddxk,ddyk,ddzk) + q ((ddxq,ddyq,ddzq)
C     RASNIK frame
      ex(1)=1
      ex(2)=0
      ex(3)=0
      ey(1)=0
      ey(2)=1
      ey(3)=0
C     goto component frame
      do i1=1,3
         exa(i1)=0.
         eya(i1)=0.
         do i2=1,3
            exa(i1)=exa(i1)+ras(30+j,i2,i1)*ex(i2)
            eya(i1)=eya(i1)+ras(30+j,i2,i1)*ey(i2)
         enddo
      enddo
C     goto global frame
      do i1=1,3
         exaa(i1)=0.
         eyaa(i1)=0.
         do i2=1,3
            kcp=rastocp(30+j)
c     print *,'should be 1 1 3 3 ',kcp
            exaa(i1)=exaa(i1)+cp(kcp,i2,i1)*exa(i2)
            eyaa(i1)=eyaa(i1)+cp(kcp,i2,i1)*eya(i2)
         enddo
      enddo

C     finalise
      xm=bilpos(j,1)
      ym=bilpos(j,2)
      zm=bilpos(j,3)
      ddxk=exaa(1)
      ddyk=exaa(2)
      ddzk=exaa(3)
      ddxq=eyaa(1)
      ddyq=eyaa(2)
      ddzq=eyaa(3)
      
C     l Vl + p (-Vk) + q (-Vq) = Vm-Vo
      V(1,1)=ddx
      V(2,1)=ddy
      V(3,1)=ddz
      V(1,2)=-ddxk
      V(2,2)=-ddyk
      V(3,2)=-ddzk
      V(1,3)=-ddxq
      V(2,3)=-ddyq
      V(3,3)=-ddzq
      
      dd(1)=xm-x0
      dd(2)=ym-y0
      dd(3)=zm-z0

      do i=1,3
         ddold(i)=dd(i)
      enddo
      call DEQN(3,V,3,IR,IFAIL,1,dd)
      if (ifail.ne.0) print *,'terug naar af ',ifail
      do i=1,3
         ddold(i)=dd(i)
      enddo
      
      dd(1)=x0+ddold(1)*ddx
      dd(2)=y0+ddold(1)*ddy
      dd(3)=z0+ddold(1)*ddz
      
C     get inverse ras
      do i1=1,3
         do i2=1,3
            rasi(i2,i1)=ras(30+j,i2,i1)
         enddo
      enddo
      
      call DINV(3,rasi,3,IR,ifail)
      if (ifail.ne.0) print *,' error rasi '
      
C     get inverse cp
      do i1=1,3
         do i2=1,3
            kcp=rastocp(30+j)
            cpi(i2,i1)=cp(kcp,i2,i1)
         enddo
      enddo
      call DINV(3,cpi,3,IR,ifail)
      if (ifail.ne.0) print *,' error cpi '
      
C     from global dd to CP dd
      ddold(1)=dd(1)-xm
      ddold(2)=dd(2)-ym
      ddold(3)=dd(3)-zm
      do i1=1,3
         dd(i1)=0.
         do i2=1,3
            dd(i1)=dd(i1)+cpi(i2,i1)*ddold(i2)
         enddo
      enddo
      
C     from CP to component
      do i1=1,3
         ddold(i1)=dd(i1)
      enddo
      do i1=1,3
         dd(i1)=0.
         do i2=1,3
            dd(i1)=dd(i1)+rasi(i2,i1)*ddold(i2)
         enddo
      enddo
      xss=dd(1)
      yss=dd(2)
      zss=dd(3)
      end
      
      subroutine cgeom(ifile)
C===============================================================
C write out the object file (obj.'ifile') based on the fitted 
C parameters.
C In this context it is usually called with ifile=-1.
C 
C===============================================================
      implicit none
      include 'mcommon'
      integer iobj,i,j,k,ifile,iosave,ii,jj
      character*80 line
      character*4 subname
      character*9 name
      double precision a,b,c,d,aa(3,3)
      double precision xx(3),xuse(100),yuse(100),zuse(100)
      double precision x(100),y(100),z(100),bb(3,3),aaa(100,3,3)
      double precision x0,y0,z0
      double precision L1,L2,L3,wxl,wxv,axb1,axb2,axb3,
     +   axl1,axl2,axl3,axv1,axv2,axv3,ax,ax1,ax2,ax3
      double precision axb1,axl1,axv1,dy1,dz1,ay1,az1,sagy1,sagz1
      double precision axb2,axl2,axv2,dy2,dz2,ay2,az2,sagy2,sagz2
      double precision axb3,axl3,axv3,dy3,dz3,ay3,az3,sagy3,sagz3
      double precision wxb1,wxl1,wxv1,gx1
      double precision wxb2,wxl2,wxv2,gx2
      double precision wxb3,wxl3,wxv3,gx3
      double precision wxay1,wxay2,wxay3
      double precision wxsz1,wxsz2,wxsz3
      double precision wxaz1,wxaz2,wxaz3
      double precision wxsy1,wxsy2,wxsy3
      double precision hcham
      double precision xc1,xc2,xc3,yc1,yc2,yc3,zc1,zc2,zc3
      double precision wxl,wxv
      double precision wxl_a,wxl_b,dz_b,dz_a,a,b
      double precision wxv_a,wxv_b,dy_b,dy_a
      double precision wzl1,wzl2,wzl3,wyl1,wyl2,wyl3
      double precision wzv1,wzv2,wzv3,wyv1,wyv2,wyv3
      double precision tnoms(3) 

      integer icha,ioff,ievtsave
      common /qwsx/x,y,z,xuse,yuse,zuse,aaa,first
      logical first
      data first /.TRUE./

      write(subname,'(I4)')ifile
      name='../obj.'//subname
      iosave=ioptions(4)
      ioptions(4)=-1.           ! special treatment sag in shift routine

c      print *,' in cgeom first ',first
      if (first) then
         open(33,file='datcha_1.obj')
         read(33,*)iobj
         do i=1,iobj
         read(33,'(a)')line
         read(33,*)a,b,c
         DO J=1,3
            read(33,*)a,b,c,d
            xx(j)=dble(d)
            aaa(i,1,j)=a
            aaa(i,2,j)=b
            aaa(i,3,j)=c
         ENDDO
        x(i)=xx(1)
	y(i)=xx(2)
	z(i)=xx(3)
        call shift(xx(1),xx(2),xx(3),xuse(i),yuse(i),zuse(i))
      enddo
      close(33)
      endif


      if (ifile.ge.0) then
         open(33,file='datcha_1.obj')
         open(34,file=name)
         read(33,*)iobj
         write(34,*)iobj
         do i=1,iobj
            read(33,'(a)')line
            write(34,*)line(1:70)
            read(33,*)a,b,c
            write(34,*)a,b,c
c            if (yuse(i).lt.6000.) then
c               read(33,*)a,b,c,d
c               write(34,*)a,b,c,(xuse(i))
c               read(33,*)a,b,c,d
c               write(34,*)a,b,c,(yuse(i))
c               read(33,*)a,b,c,d
c               write(34,*)a,b,c,(zuse(i))
c            else
               icha=1
               if (yuse(i).gt.6000.) icha=2
               if (yuse(i).gt.8500.) icha=3
               
               if (icha.eq.2) ioff=0
               if (icha.eq.3) ioff=9
               
               xn(icha)=x(i)
               yn(icha)=y(i)
               zn(icha)=z(i)
               
      xc1=0.
      zc1=zcham(1)
      yc1=ycham(1)
      xc2=0.
      zc2=zcham(2)
      yc2=ycham(2)
      xc3=0.
      zc3=zcham(3)
      yc3=ycham(3)


               tnoms(1)=tnom(1)
               tnoms(2)=tnom(2)
               tnoms(3)=tnom(3)
               tnom(1)=-2.05
               tnom(2)=-2.05
               tnom(3)=-2.05
               ievtsave=ievt
               ievt=nevt+1
               amdt(ievt,icha)=zn(icha)/yn(icha)
               arpc(ievt)=xn(icha)/yn(icha)
c               print *,xn(1),amdt(ievt,icha),arpc(ievt)
               include 'wxfunc.f'
c               print *,'after wxfun'
               ievt=ievtsave
               tnom(1)=tnoms(1)
               tnom(2)=tnoms(2)
               tnom(3)=tnoms(3)
           

               axb1=0.
               axl1=0.
               axv1=rfits(1)
               axb2=rfits(10)
               axl2=rfits(11)
               axv2=rfits(12)
               axb3=rfits(19)
               axl3=rfits(20)
               axv3=rfits(21)
c               print *,xn(1),xcham(1)
c               print *,wxb1,wxl1,wxv1
c               print *,axb1,axl1,axv1
c               print *,'---------===============----------'
               ax1=axb1*wxb1+axl1*wxl1+axv1*wxv1
               ax2=axb2*wxb2+axl2*wxl2+axv2*wxv2
               ax3=axb3*wxb3+axl3*wxl3+axv3*wxv3
               
               ax=ax1
               if (icha.eq.2) ax=ax2
               if (icha.eq.3) ax=ax3
               aa(1,1)=1.
               aa(1,2)=rfits(ioff+14)*-1.
     +                 + 0.*2.*rfits(18+ioff)/(xcham(icha)/2.)
     +                                  *x(i)/(xcham(icha)/2)
               aa(1,3)=-rfits(ioff+13) -      
     +                    0.* 2.*rfits(17+ioff)/(xcham(icha)/2.)
     +                                  *x(i)/(xcham(icha)/2)
               aa(2,1)=-aa(1,2)
               aa(2,2)=1.
               aa(2,3)=ax
               aa(3,1)=-aa(1,3)
               aa(3,2)=-aa(2,3)
               aa(3,3)=1.
               
               if (icha.eq.1) then
                  aa(1,1)=1.
                  aa(1,2)=0.
                  aa(1,3)=0.
                  aa(2,1)=0.
                  aa(2,2)=1.
                  aa(2,3)=ax
                  aa(3,1)=0.
                  aa(3,2)=-ax
                  aa(3,3)=1.
               endif
               read(33,*)a,b,c,d
               write(34,*)aa(1,1),aa(1,2),aa(1,3),(xuse(i))
               read(33,*)a,b,c,d
               write(34,*)aa(2,1),aa(2,2),aa(2,3),(yuse(i))
               read(33,*)a,b,c,d
               write(34,*)aa(3,1),aa(3,2),aa(3,3),(zuse(i))
c            endif
         enddo
         close(33)
         close(34)
      else
         do i=1,iobj
               icha=1
               if (yuse(i).gt.6000.) icha=2
               if (yuse(i).gt.8500.) icha=3
               ioff=0
               if (icha.eq.2) ioff=0
               if (icha.eq.3) ioff=9
               
               xn(icha)=x(i)
               yn(icha)=y(i)
               zn(icha)=z(i)
               
      xc1=0.
      zc1=zcham(1)
      yc1=ycham(1)
      xc2=0.
      zc2=zcham(2)
      yc2=ycham(2)
      xc3=0.
      zc3=zcham(3)
      yc3=ycham(3)


               ievtsave=ievt
               tnoms(1)=tnom(1)
               tnoms(2)=tnom(2)
               tnoms(3)=tnom(3)
               tnom(1)=-2.05
               tnom(2)=-2.05
               tnom(3)=-2.05
               ievt=nevt+1
               amdt(ievt,icha)=zn(icha)/yn(icha)
               arpc(ievt)=xn(icha)/yn(icha)
c               print *,xn(1),amdt(ievt,icha),arpc(ievt)
               include 'wxfunc.f'
c               print *,'after wxfun'
               ievt=ievtsave
               tnom(1)=tnoms(1)
               tnom(2)=tnoms(2)
               tnom(3)=tnoms(3)
               
               axb1=0.
               axl1=0.
               axv1=rfits(1)
               axb2=rfits(10)
               axl2=rfits(11)
               axv2=rfits(12)
               axb3=rfits(19)
               axl3=rfits(20)
               axv3=rfits(21)
c               print *,xn(1),xcham(1)
c               print *,wxb1,wxl1,wxv1
c               print *,axb1,axl1,axv1
c               print *,'---------===============----------'
               ax1=axb1*wxb1+axl1*wxl1+axv1*wxv1
               ax2=axb2*wxb2+axl2*wxl2+axv2*wxv2
               ax3=axb3*wxb3+axl3*wxl3+axv3*wxv3
               
               ax=ax1
               if (icha.eq.2) ax=ax2
               if (icha.eq.3) ax=ax3
               aa(1,1)=1.
               aa(1,2)=rfits(ioff+14)*-1.
     +                 + 0.*2.*rfits(18+ioff)/(xcham(icha)/2.)
     +                                  *x(i)/(xcham(icha)/2)
               aa(1,3)=-rfits(ioff+13) -      
     +                    0.* 2.*rfits(17+ioff)/(xcham(icha)/2.)
     +                                  *x(i)/(xcham(icha)/2)
               aa(2,1)=-aa(1,2)
               aa(2,2)=1.
               aa(2,3)=ax
               aa(3,1)=-aa(1,3)
               aa(3,2)=-aa(2,3)
               aa(3,3)=1.
               
               if (icha.eq.1) then
                  aa(1,1)=1.
                  aa(1,2)=0.
                  aa(1,3)=0.
                  aa(2,1)=0.
                  aa(2,2)=1.
                  aa(2,3)=ax
                  aa(3,1)=0.
                  aa(3,2)=-ax
                  aa(3,3)=1.
               endif
               do ii=1,3
                  do jj=1,3
                     cp(i,ii,jj)=aa(jj,ii)
                  enddo
               enddo   
               cppos(i,1)=xuse(i)
               cppos(i,2)=yuse(i)
               cppos(i,3)=zuse(i)

               if (debug.eq.3) then
c     test
                  write(*,*)'----------------------'
                  write(*,*)aa(1,1),aa(1,2),aa(1,3),real(xuse(i))
                  write(*,*)aa(2,1),aa(2,2),aa(2,3),real(yuse(i))
                  write(*,*)aa(3,1),aa(3,2),aa(3,3),real(zuse(i))
               endif
               

c            endif
         enddo
      endif

c      print *,'after cgeom Bil ',cppos(1,1),cppos(1,2),cppos(1,3)

c      call shift(cppos(2,1),cppos(2,2),cppos(2,3),
c     +     x0,y0,z0)
c      print *,'BIL CP ',x0,y0,z0
c      call shift(-1280.d0,4883.d0,-2581.286d0,x0,y0,z0)
c      print *,'BIL CP MASK0 ',x0,y0,z0
      
      ioptions(4)=iosave
      end







      subroutine cgeomold(ifile)
C===============================================================
C write out the object file (obj.'ifile') based on the fitted 
C parameters.
C In this context it is usually called with ifile=-1.
C 
C===============================================================
      implicit none
      include 'mcommon'
      integer iobj,i,j,k,ifile,iosave,ii,jj
      character*80 line
      character*4 subname
      character*9 name
      double precision a,b,c,d,aa(3,3)
      double precision xx(3),xuse(100),yuse(100),zuse(100)
      double precision x(100),y(100),z(100),bb(3,3),aaa(100,3,3)
      double precision x0,y0,z0
      common /qwsx/x,y,z,xuse,yuse,zuse,aaa,first
      logical first
      data first /.TRUE./

      write(subname,'(I4)')ifile
      name='../obj.'//subname
      iosave=ioptions(4)
      ioptions(4)=-1.           ! special treatment sag in shift routine

c      print *,' in cgeom first ',first
      if (first) then
         open(33,file='datcha_1.obj')
         read(33,*)iobj
         do i=1,iobj
         read(33,'(a)')line
         read(33,*)a,b,c
         DO J=1,3
            read(33,*)a,b,c,d
            xx(j)=dble(d)
            aaa(i,1,j)=a
            aaa(i,2,j)=b
            aaa(i,3,j)=c
         ENDDO
        x(i)=xx(1)
	y(i)=xx(2)
	z(i)=xx(3)
        call shift(xx(1),xx(2),xx(3),xuse(i),yuse(i),zuse(i))
      enddo
      close(33)
      endif


      if (ifile.ge.0) then
         open(33,file='datcha_1.obj')
         open(34,file=name)
         read(33,*)iobj
         write(34,*)iobj
         do i=1,iobj
            read(33,'(a)')line
            write(34,*)line(1:70)
            read(33,*)a,b,c
            write(34,*)a,b,c
c            if (yuse(i).lt.6000.) then
c               read(33,*)a,b,c,d
c               write(34,*)a,b,c,(xuse(i))
c               read(33,*)a,b,c,d
c               write(34,*)a,b,c,(yuse(i))
c               read(33,*)a,b,c,d
c               write(34,*)a,b,c,(zuse(i))
c            else
               call shift(x(i)+1.d0,y(i),z(i),
     +              bb(1,1),bb(2,1),bb(3,1) )
c     print *,'test ',bb(1,1),bb(2,1),bb(3,1),
c     +                   xuse(i),yuse(i),zuse(i)
               call shift(x(i),y(i)+1.d0,z(i),
     +              bb(1,2),bb(2,2),bb(3,2) )
               call shift(x(i),y(i),z(i)+1.d0,
     +              bb(1,3),bb(2,3),bb(3,3) )
	       do k=1,3
                  aa(1,k)=bb(1,k)-xuse(i)
                  aa(2,k)=bb(2,k)-yuse(i)
                  aa(3,k)=bb(3,k)-zuse(i)
	       enddo
c     print *,'t1 ',bb(1,1),xuse(i),aa(1,1)
               read(33,*)a,b,c,d
               write(34,*)aa(1,1),aa(1,2),aa(1,3),(xuse(i))
               read(33,*)a,b,c,d
               write(34,*)aa(2,1),aa(2,2),aa(2,3),(yuse(i))
               read(33,*)a,b,c,d
               write(34,*)aa(3,1),aa(3,2),aa(3,3),(zuse(i))
c            endif
         enddo
         close(33)
         close(34)
      else
         do i=1,iobj
c            if (yuse(i).lt.6000.) then
c               do ii=1,3
c                  do jj=1,3
c                     cp(i,ii,jj)=aaa(i,ii,jj)
c                  enddo
c               enddo
c               cppos(i,1)=xuse(i)
c               cppos(i,2)=yuse(i)
c               cppos(i,3)=zuse(i)
c            else
               call shift(x(i)+1.d0,y(i),z(i),
     +              bb(1,1),bb(2,1),bb(3,1) )
               call shift(x(i),y(i)+1.d0,z(i),
     +              bb(1,2),bb(2,2),bb(3,2) )
               call shift(x(i),y(i),z(i)+1.d0,
     +              bb(1,3),bb(2,3),bb(3,3) )

	       do k=1,3
                  aa(1,k)=bb(1,k)-xuse(i)
                  aa(2,k)=bb(2,k)-yuse(i)
                  aa(3,k)=bb(3,k)-zuse(i)
	       enddo
C martin test
               aa(2,1)=-aa(1,2)
               aa(3,1)=-aa(1,3)               

               do ii=1,3
                  do jj=1,3
                     cp(i,ii,jj)=aa(jj,ii)
                  enddo
               enddo   
               cppos(i,1)=xuse(i)
               cppos(i,2)=yuse(i)
               cppos(i,3)=zuse(i)

               if (debug.eq.3) then
c     test
                  write(*,*)'----------------------'
                  write(*,*)aa(1,1),aa(1,2),aa(1,3),real(xuse(i))
                  write(*,*)aa(2,1),aa(2,2),aa(2,3),real(yuse(i))
                  write(*,*)aa(3,1),aa(3,2),aa(3,3),real(zuse(i))
               endif
               

c            endif
         enddo
      endif

c      print *,'after cgeom Bil ',cppos(1,1),cppos(1,2),cppos(1,3)

c      call shift(cppos(2,1),cppos(2,2),cppos(2,3),
c     +     x0,y0,z0)
c      print *,'BIL CP ',x0,y0,z0
c      call shift(-1280.d0,4883.d0,-2581.286d0,x0,y0,z0)
c      print *,'BIL CP MASK0 ',x0,y0,z0
      
      ioptions(4)=iosave
      end





      subroutine fitfcn
      implicit none
C --
C base routine to fit track segments.
C See FCN for Chi2.
C Special input:
C ioptions(1)=1 --> do a MINOS call and do the histo filling in fcn
C ioptions(3)=1 --> used for fixing and releasing parameters
C --
      include 'mcommon'

C--
C rfits(1) step size for dz of BML chamber
C rfits(2) step size for dz of BOL chamber
      
      integer ierr,ier

      double precision b1(50),b2(50),arg(10)
      double precision stval(30),step(30)
      character*10 chnam(30),naam
      integer ii,iii,i
      external FCN
      
      print *,'FIT FCN==================================== '

      i=1
      call mncomd(fcn,'SET PRI 0',i,0)

      
      chnam(1)='axb1'
      chnam(2)='axl1'
      chnam(3)='axv1'
      chnam(4)='ay1'
      chnam(5)='az1'
      chnam(6)='dy1'
      chnam(7)='dz1'
      chnam(8)='sagz1'
      chnam(9)='sagy1'

      chnam(10)='axb2'
      chnam(11)='axl2'
      chnam(12)='axv2'
      chnam(13)='ay2'
      chnam(14)='az2'
      chnam(15)='dy2'
      chnam(16)='dz2'
      chnam(17)='sagz2'
      chnam(18)='sagy2'

      chnam(19)='axb3'
      chnam(20)='axl3'
      chnam(21)='axv3'
      chnam(22)='ay3'
      chnam(23)='az3'
      chnam(24)='dy3'
      chnam(25)='dz3'
      chnam(26)='sagz3'
      chnam(27)='sagy3'
	chnam(28)='dxbml'
        chnam(29)='dtemp'

      do i=1,30
         stval(i)=rfits(i)
         step(i)=0.1
         naam=chnam(i)
         b1(i)=-30.
         b2(i)=30.
         if (naam(1:1).eq.'a') then
            step(i)=0.0005      ! angle
            b1(i)=-0.1
            b2(i)=0.1
         endif
      enddo
      stval(22)=0.

      if (ifs.eq.4) then
         do i=1,9
            step(i)=0.
         enddo
         step(11)=0.
         step(12)=0.
         do i=17,18
            step(i)=0.
         enddo
         step(20)=0.
         step(21)=0.
         step(26)=0.
         step(27)=0.
C set BML to zero     
c          do i=1,18
c             if (i.ne.13) then
c                step(i)=0.
c             endif
c           enddo
C set BOL to zero
           do i=19,27
              if (i.ne.22) then
              STEP(I)=0.
              endif
           enddo
	step(29)=0.
C
      elseif (ifs.eq.1) then
        do i=10,29
           step(i)=0.
        enddo
        step(3)=0.
        do i=4,7
           step(i)=0.
        enddo
      elseif (ifs.eq.2) then
        do i=1,10
           step(i)=0.
        enddo
        do i=19,29
           step(i)=0.
        enddo
	step(13)=0.
        step(14)=0.
	step(15)=0.
        step(16)=0.
      elseif (ifs.eq.3) then
        do i=1,19
           step(i)=0.
        enddo
        do i=22,25
           step(i)=0.
        enddo
        step(28)=0.
        step(29)=0.
      endif


      arg(1)=0.d0
      do  i=1,29
         call mnparm(i,chnam(i),stval(i),step(i),b1(i),b2(i),ier)   
      enddo

      if (ioptions(1).ne.-1) then
c     call mnexcm(fcn,'SIMPLEX',arg,0,ierr,0)
      call mnexcm(fcn,'MINIMIZE',arg,0,ierr,0)
      call mnexcm(fcn,'CALL FCN',arg,0,ierr,0)     

      if (ifs.eq.3) then
         call mnexcm(fcn,'MINOS',arg,0,ierr,0)     
         call mnexcm(fcn,'SHOCOR',arg,0,ierr,0)     
         call mnexcm(fcn,'SHOCOV',arg,0,ierr,0)     
      endif

      if (ioptions(1).eq.1) then
         call mncomd(fcn,'SET PRI 1',i,0)
         call mnexcm(fcn,'MINOS',arg,0,ierr,0)     
         call mnexcm(fcn,'SHOCOR',arg,0,ierr,0)     
         call mnexcm(fcn,'SHOCOV',arg,0,ierr,0)     
         ioptions(1)=2
         call mnexcm(fcn,'CALL FCN',arg,0,ierr,0)     
         ioptions(1)=1
      endif
      endif
      end


      subroutine fcn(npar,gin,chi2,xxx,iflag)
*************************************************************************
* Chi2 for RASNIK readings. 
* With 'rfits' a new object file is created by cgeom.
* It is read in by initras and then used in preras to
* predict new RASNIK values.
*************************************************************************
      implicit none
      include 'mcommon'

      double precision xxx(*),Gin(*),chi2
      integer iflag,i,npar,isys
      double precision chix,chiz,chim,chir

      do i=1,29
            rfits(i)=xxx(i)
      enddo
      rfits(22)=rfitssave(22)+xxx(22)*0.906
      rfits(23)=rfitssave(23)+xxx(22)*0.423

      isys=ifs
      call cgeom(-1)
      call initras(-1)
      call predict(isys)

      chi2=0.d0
      chix=0.d0
      chiz=0.d0
      chim=0.d0
      chir=0.d0
      do i=1,4
         chix=chix+ (xras(i)-xrasin(igo,isys,i))**2 / 
     +                exrasin(igo,isys,i)**2
         chiz=chiz+ (yras(i)-yrasin(igo,isys,i))**2 / 
     +                eyrasin(igo,isys,i)**2
         chim=chim+ (dmras(i)-dmrasin(igo,isys,i))**2 /
     +                edmrasin(igo,isys,i)**2
         chir=chir+ (rotras(i)-rotrasin(igo,isys,i))**2 /
     +                erotrasin(igo,isys,i)**2
      enddo
      chi2=chix+chiz+chim+chir
      end




      subroutine fitcal
      implicit none
C --
C fit rasnik calibration
C --
      include 'mcommon'

C--
      
      integer ierr,ier

      double precision b1(50),b2(50),arg(10)
      double precision stval(30),step(30)
      character*10 chnam(30),naam
      integer ii,iii,i,isys
      external FCNCAL
      
      print *,'FIT FCNCAL==================================== '

      i=1
      call mncomd(fcncal,'SET PRI 0',i,0)

      
      chnam(1)='Cy'
      chnam(2)='Cx'
      chnam(3)='Cm'
      chnam(4)='aa'
      chnam(5)='as'
      
      isys=ifs
      do i=1,5
         stval(i)=offset(isys,jco,i)
         step(i)=1.
c         IF (I.GT.2) step(i)=0.
         b1(i)=-100.
         b2(i)=100.
      enddo
      
      arg(1)=0.d0
      do  i=1,5
         call mnparm(i,chnam(i),stval(i),step(i),b1(i),b2(i),ier)   
      enddo

      call mnexcm(fcncal,'MINIMIZE',arg,0,ierr,0)
      call mnexcm(fcncal,'CALL FCN',arg,0,ierr,0)     
c      call mncomd(fcncal,'SET PRI 1',i,0)
c      call mnexcm(fcncal,'MINOS',arg,0,ierr,0)     
      end


      subroutine fcncal(npar,gin,chi2,xxx,iflag)
*************************************************************************
* Chi2 for RAW RASNIK readings, to get calibration. 
* With 'rfits' a new object file is created by cgeom.
* It is read in by initras and then used in preras to
* predict new RASNIK values.
*************************************************************************
      implicit none
      include 'mcommon'

      double precision xxx(*),Gin(*),chi2
      integer iflag,i,npar,isys,j,i1,ibingo
      double precision chix,chiz,chim,chir
      double precision x,y,z,xi,yi,zi,xr,yr,zr
      isys=ifs
      do i=1,5
         offset(isys,jco,i)=xxx(i)
      enddo


      if (isys.ne.4) then
         print *,'sys ne 4 in fcncal '
         stop
      endif


      chi2=0.d0
      chix=0.d0
      chiz=0.d0
      chim=0.d0
      chir=0.d0

      do j=1,nruns
         ibingo=0
         do i1=1,irun
            if (run(i1).eq.iorder(j)) ibingo=i1
         enddo
         if (ibingo.eq.0) print *,' NONSENS CALIBRATION in fcncal'
         call initras(isuffix(j))
         call predict(isys)
         
         do i=jco,jco

            x=xras(i)
            y=yras(i)
            z=0.d0
            call toglobmsk(isys,i,x,y,z,xr,yr,zr)
            x=xrasin(ibingo,isys,i)
            y=yrasin(ibingo,isys,i)
            z=0.d0
            call toglobmsk(isys,i,x,y,z,xi,yi,zi)
            chix=(yi-yr)**2/xrasin(ibingo,isys,i)**2
            chix=chix+(xi-xr)**2/xrasin(ibingo,isys,i)**2
            chiz=(zi-zr)**2/yrasin(ibingo,isys,i)**2
c            chix=chix+ (xras(i)-xrasin(ibingo,isys,i))**2 / 
c     +           exrasin(ibingo,isys,i)**2
c            chiz=chiz+ (yras(i)-yrasin(ibingo,isys,i))**2 / 
c     +           eyrasin(ibingo,isys,i)**2
            chim=chim+ (dmras(i)-dmrasin(ibingo,isys,i))**2 /
     +           edmrasin(ibingo,isys,i)**2
            chir=chir+ (rotras(i)-rotrasin(ibingo,isys,i))**2 /
     +           erotrasin(ibingo,isys,i)**2
         enddo
      enddo
      chi2=chix+chiz+chim+chir
      end


      
      subroutine torotras(theta,x,y,xp,yp)
C rotate
      implicit none
      include 'mcommon'
      double precision x,y,z,xp,yp,zp,xx,yy,zz,theta
      xx=x
      yy=y
      zz=z
      xp=xx*cos(theta)-yy*sin(theta)
      yp=xx*sin(theta)+yy*cos(theta)
      end
     

      subroutine torot(x,y,z,xp,yp,zp)
C rotate
      implicit none
      include 'mcommon'
      double precision x,y,z,xp,yp,zp,xx,yy,zz
      xx=x
      yy=y
      zz=z
      xp=xx
      yp=yy*ctf-zz*stf
      zp=yy*stf+zz*ctf
      end
     
      subroutine toold(x,y,z,xp,yp,zp)
C unrotate
      implicit none
      include 'mcommon'
      double precision x,y,z,xp,yp,zp,xx,yy,zz
      xx=x
      yy=y
      zz=z
      xp=xx
      yp=yy*ctf+zz*stf
      zp=zz*ctf-yy*stf
      end


      subroutine shift(xx,yy,zz,xuse,yuse,zuse)
      implicit none
      include 'mcommon'
      double precision xx,yy,zz,xuse,yuse,zuse
      integer icha,i,ievtsave
      double precision tnoms(3)
      icha=1
      if (yy.gt.6000.) icha=2
      if (yy.gt.8500.) icha=3
      xn(icha)=xx
      yn(icha)=yy
      zn(icha)=zz

      tnoms(1)=tnom(1)
      tnoms(2)=tnom(2)
      tnoms(3)=tnom(3)
      tnom(1)=-2.05
      tnom(2)=-2.05
      tnom(3)=-2.05
      ievtsave=ievt
      ievt=1
      amdt(ievt,icha)=zz/yy
      arpc(ievt)=xx/yy
      call shiftnew
      xuse=xs(icha)
      yuse=ys(icha)
      zuse=zs(icha)
      tnom(1)=tnoms(1)
      tnom(2)=tnoms(2)
      tnom(3)=tnoms(3)
      ievt=ievtsave
      end

      subroutine shiftnew
      implicit none 
      include 'mcommon'
      double precision axb1,axl1,axv1,dy1,dz1,ay1,az1,sagy1,sagz1
      double precision axb2,axl2,axv2,dy2,dz2,ay2,az2,sagy2,sagz2
      double precision axb3,axl3,axv3,dy3,dz3,ay3,az3,sagy3,sagz3
      double precision tn,ttn,stn,ctn,tn1,tn2,tn3
ccc      double precision xn1,xn2,xn3,yn1,yn2,yn3,zn1,zn2,zn3
      double precision L1,L2,L3
      double precision xc1,xc2,xc3,yc1,yc2,yc3,zc1,zc2,zc3
      double precision wxb1,wxl1,wxv1,gx1
      double precision wxb2,wxl2,wxv2,gx2
      double precision wxb3,wxl3,wxv3,gx3
      double precision wxay1,wxay2,wxay3
      double precision wxsz1,wxsz2,wxsz3
      double precision wxaz1,wxaz2,wxaz3
      double precision wxsy1,wxsy2,wxsy3
      double precision hcham,dtemp
      double precision wxl,wxv
      double precision wxl_a,wxl_b,dz_b,dz_a,a,b
      double precision wxv_a,wxv_b,dy_b,dy_a
      double precision wzl1,wzl2,wzl3,wyl1,wyl2,wyl3
      double precision wzv1,wzv2,wzv3,wyv1,wyv2,wyv3
      integer i
      hcham=50.

c      do i=1,50
c         rfitssave(i)=rfits(i)
c      enddo

c      rfits(22)=rfitssave(22)*0.906 - rfitssave(23)*0.423
c      rfits(23)=rfitssave(22)*0.423 + rfitssave(23)*0.906
c      print *,'marcel test',rfits(22),rfits(23)

c     if (ioptions(4).gt.0) then
         axb1=0.
         axl1=0.
         axv1=rfits(1)
         ay1=0.
         az1=0.
         dy1=0.
         dz1=0.
         sagz1=0.
         sagy1=0.
c     else
c        axb1=rfits(3)
c        axl1=rfits(2)
c        axv1=rfits(1)
c        ay1=rfits(4)
c        az1=rfits(5)
c        dy1=rfits(6)
c        dz1=rfits(7)
c        sagz1=rfits(8)
c        sagy1=rfits(9)
c     endif

      axb2=rfits(10)
      axl2=rfits(11)
      axv2=rfits(12)
      ay2=rfits(13)
      az2=rfits(14)
      dy2=rfits(15)
      dz2=rfits(16)
      sagz2=rfits(17)
      sagy2=rfits(18)

      axb3=rfits(19)
      axl3=rfits(20)
      axv3=rfits(21)
      ay3=rfits(22)
      az3=rfits(23)
      dy3=rfits(24)
      dz3=rfits(25)
      sagz3=rfits(26)
      sagy3=rfits(27)

      dtemp=rfits(29)

      xc1=0.
      zc1=zcham(1)
      yc1=ycham(1)
      xc2=0.
      zc2=zcham(2)
      yc2=ycham(2)
      xc3=0.
      zc3=zcham(3)
      yc3=ycham(3)




      tn=atan((yn(3)-yn(1))/(zn(3)-zn(1)))
      ttn=tan(tn)
      stn=sin(tn)
      ctn=cos(tn)

      include 'wxfunc.f'

c      print *,wzv1,wyv1




      zs(1)=zn(1)+dz1+ay1*(xn(1)-xc1) -(axb1*wxb1)
     +     *(yn(1)-yc1)-
     +       (axl1*wzl1+axv1*wzv1)
     +      +sagz1*gx1
      zs(2)=zn(2)+dz2+ay2*(xn(2)-xc2) -(axb2*wxb2)
     +     *(yn(2)-yc2) -
     +      (axl2*wzl2+axv2*wzv2)      
     +       +sagz2*gx2
      zs(3)=zn(3)+dz3+ay3*(xn(3)-xc3) -
     +      (axb3*wxb3)*(yn(3)-yc3)
     +      -(axl3*wzl3+axv3*wzv3)
     +      +sagz3*gx3


      ys(1)=yn(1)+dy1+az1*(xn(1)-xc1)+axb1*(zn(1)-zc1)*wxb1+
     +            axl1
     +            *wyl1+axv1*wyv1+sagy1*gx1
      ys(2)=yn(2)+dy2+az2*(xn(2)-xc2)+axb2*(zn(2)-zc2)*wxb2+
     +             axl2
     +            *wyl2+axv2*wyv2+sagy2*gx2
      ys(3)=yn(3)+dy3+az3*(xn(3)-xc3)+axb3*(zn(3)-zc3)*wxb3+
     +            axl3
     +            *wyl3+axv3*wyv3+sagy3*gx3

c      print *,' sagy3 gx3 ',sagy3,gx3

c      print *,zs(1),zs(2),zs(3)
c      print *,ys(1),ys(2),ys(3)

c      print *,'yn yc ',real(arpc(ievt)),real(zn(2)),real(zc2)
c      print *,'a w',real(axl2),real(wxl2)
c      print *,'change in y naif 3 ',
c     +              real((axl2*wxl2)*(zn(2)-zc2))
c      print *,'change in y full 2 ',real((axl2*wyl2))
c      print *,'-----------'
      tn1=tnom(1)
      tn2=tnom(2)
      tn3=tnom(3)

      ts(1)=tn1+axb1*wxb1+axl1*wxl1+axv1*wxv1+ay1*wxay1+
     +     sagz1*wxsz1+az1*wxaz1+sagy1*wxsy1
      ts(2)=tn2+axb2*wxb2+axl2*wxl2+axv2*wxv2+ay2*wxay2+
     +     sagz2*wxsz2+az2*wxaz2+sagy2*wxsy2
      ts(3)=tn3+axb3*wxb3+axl3*wxl3+axv3*wxv3+ay3*wxay3+
     +     sagz3*wxsz3+az3*wxaz3+sagy3*wxsy3


      xs(1)=xn(1)
      xs(2)=xn(2)+rfits(28)
      xs(3)=xn(3)


      if (ioptions(4).eq.-1) then ! call from cgeom
         xs(1)=xs(1)-
     +        (az1+0.*sagy1/(L1/2)*(xn(1)/(L1/2)))*(yn(1)-yc1)-
     +        (ay1+0.*sagz1/(L1/2)*(xn(1)/(L1/2)))*(zn(1)-zc1)
         xs(2)=xs(2)-
     +        (az2+0.*sagy2/(L2/2)*(xn(2)/(L2/2)))*(yn(2)-yc2)-
     +        (ay2+0.*sagz2/(L2/2)*(xn(2)/(L2/2)))*(zn(2)-zc2)
         xs(3)=xs(3)-
     +        (az3+0.*sagy3/(L3/2)*(xn(3)/(L3/2)))*(yn(3)-yc3)-
     +        (ay3+0.*sagz3/(L3/2)*(xn(3)/(L3/2)))*(zn(3)-zc3)
      endif

c      do i=1,50
c         rfits(i)=rfitssave(i)
c      enddo

C try to include a hoimogenius temp expansion
      zs(2)=zs(2)+0.025*(zn(2)-zcham(2))*dtemp
      xs(2)=xs(2)+0.025*(xn(2))*dtemp


      end
      
      double precision function wxl(x,L)
      implicit none
      double precision x,L
      wxl=((-abs(x)+L/2)/(L/2))
      end

      double precision function wxv(x,L)
      implicit none
      double precision x,L
      wxv=0.5*((-x+L/2)/(L/2))-0.5*((x+L/2)/(L/2))      
      end

      
      subroutine getsagittag(sag,ishift)
***************************************************************
* calculate sagitta. Baseline from point defined by           *
* local track and ycenter BIL and BOL.                        *            
* Then distance from this line to point defined by local      *
* track and ycenter BML.                                      * 
*                                                             *
* input: ishift=0 use standard geometry.                      * 
*        ishift=1 use shifted=result-from-fit geometry        * 
*                                                             *
* output: sag=sagitta                                         *
***************************************************************
      implicit none
      include 'mcommon'

      
      real sag
      integer ishift,i

      double precision yy1,yy2,yy3,zz1,zz2,zz3,
     +                 ybil,ybml,ybol,zbil,zbml,zbol,
     +  xx1,xx2,xx3,xbil,xbml,xbol
      double precision dzdy,b,a,ap,bp,yl,zl,d

      yy1=ycham(1)
      yy2=ycham(2)
      yy3=ycham(3)

      zz1=zcham(1)
      zz2=zcham(2)
      zz3=zcham(3)
     
      xx1=0.d0
      xx2=0.d0
      xx3=0.d0
      
      if (ishift.eq.0) then
         xbil=xx1
         xbml=xx2
         xbol=xx3
         ybil=yy1
         ybml=yy2
         ybol=yy3
         zbil=zz1
         zbml=zz2
         zbol=zz3
      else
         call shift(xx1,yy1,zz1,xbil,ybil,zbil)
         call shift(xx2,yy2,zz2,xbml,ybml,zbml)
         call shift(xx3,yy3,zz3,xbol,ybol,zbol)

         print *,'ttag ',xbml,
     +            ybml-ycham(2),
     +            zbml-zcham(2)
      endif
      dzdy=(zbol-zbil)/(ybol-ybil)
      b=zbil-dzdy*ybil
      a=dzdy
      
C     closest approach z= a y + b
      if (a.ne.0.d0) then
         ap=-1.d0/a
      else
         ap=999999.d0
         print *,'WARININ getsag a=0 '
      endif
      bp=zbml-ap*ybml
      if (a-ap.ne.0.d0) then
         yl=(bp-b)/(a-ap)
      else
         yl=999999.d0
         print *,'WARININ getsag a-ap=0 '
      endif
      zl=a*yl+b
      if (abs(zl - (ap*yl+bp)).gt.0.1) 
     +     print *,'SHIT getsag ',i,zl,(ap*yl+bp)
      d=sqrt( (yl-ybml)**2 + (zl-zbml)**2 )
      if (zl.lt.zbml) d=-d
      sag=d
      end


      integer function idgive(icha,xx,zz)
C returns an histo id for plotting
      integer icha
      double precision xx,zz,dx(3),dz(3)
      include 'mcommon'

      dx(1)=xcham(1)
      dx(2)=xcham(2)
      dx(3)=xcham(3)
      dz(1)=960.
      dz(2)=1440.
      dz(3)=2160.
C-----------
C 1 2 3
C 4 5 6
C 7 8 9
C----------
      iz=2
      if (zz.gt.zcham(icha)+dz(icha)/2.-dz(icha)/3.) iz=1
      if (zz.lt.zcham(icha)+dz(icha)/2.-2.*dz(icha)/3.) iz=3
      ix=2
      if (xx.lt.-dx(icha)/2.+dx(icha)/3.) ix=1
      if (xx.gt.-dx(icha)/2.+2.*dx(icha)/3.) ix=3
      idgive=ix+(3-iz)*3

c      print *,'xx zz iz ix ',real(xx),real(zz),iz,ix
      end

      



      subroutine sag2d
C===========================================================
* Get sagitta as function of postion on BML plane.
* Also errors are calculated from MINOS information.
C===========================================================
      implicit none
      include 'mcommon'
      integer nx,nz,i,j
      real x2,z2,sag
      double precision xx2,zz2,yy2,xx,yy,zz
      double precision yya,yyb,yyc,yyd,epartn
      real dxbin,dzbin,x1,z1
      nx=10
      nz=5
      call hbook2(191,'sag bml 0',nx,-real(xcham(2))/2.,
     +             real(xcham(2)/2.),nz,-1440./2.,1440./2.,0.)

      call hbook2(192,'sag bml 1',nx,-real(xcham(2))/2.,
     +            real(xcham(2)/2.),nz,-1440./2.,1440./2.,0.)
      call hbook2(193,'serror',nx,-real(xcham(2))/2.,
     +            real(xcham(2)/2.),nz,-1440./2.,1440./2.,0.)
      call hbook2(194,'y shift',nx,-real(xcham(2))/2.,
     +            real(xcham(2)/2.),nz,-1440./2.,1440./2.,0.)

c      rfits(1)=0.d0
c      rfits(2)=0.d0
c      rfits(3)=0.d0
c      rfits(4)=0.d0
      xx2=-real(xcham(2))
      yy2=ycham(2)
      zz2=zcham(2)+1440./2.
      call shift(xx2,yy2,zz2,xx,yy,zz)
      yya=xx
      xx2=real(xcham(2))
      zz2=zcham(2)+1440./2.
      call shift(xx2,yy2,zz2,xx,yy,zz)
      yyb=xx
      xx2=-real(xcham(2))
      zz2=zcham(2)-1440./2.
      call shift(xx2,yy2,zz2,xx,yy,zz)
      yyc=xx
      xx2=real(xcham(2))
      zz2=zcham(2)-1440./2.
      call shift(xx2,yy2,zz2,xx,yy,zz)
      yyd=xx
      call fouten
      call hijxy(191,1,2,x1,z1)
      call hijxy(191,2,2,x2,z2)
      dxbin=x2-x1
      dzbin=z2-z1
      do i=1,nx
         do j=1,nz
            yy2=ycham(2)
            call hijxy(191,i,j,x2,z2)
            x2=x2+dxbin/2.
            z2=z2+dzbin/2.
            xx2=0. + dble(x2)
            zz2=zcham(2) + dble(z2)
            call getsagittabin(sag,0,xx2,zz2)
            call hfill(191,x2,z2,sag)
            call getsagittabin(sag,1,xx2,zz2)
c            if (debug.ge.1) then
c               print *,'sagbin ',xx2,zz2,sag
c            endif
c               print *,'sagbin standard ',x2,z2,sag
            call hfill(192,x2,z2,sag)
            call shift(xx2,yy2,zz2,xx,yy,zz)
            call dpartn(xx2,zz2,epartn)
            call hfill(193,x2,z2,real(epartn))
c            dx=xx-xx2
cc            dy=yy-yy2
c            dz=zz-zz2
c            xx2=xx2-dx
cc            yy2=yy2-dy
c            zz2=zz2-dz
c            call shift(xx2,yy2,zz2,xx,yy,zz)
            call hfill(194,x2,z2,real(yy))
         enddo
      enddo

c      xx2=-1881.5
c      zz2=-3231.714

      xx2= -1881.8749643513
      zz2=-3229.46253126082

      call getsagittabin(sag,1,xx2,zz2)
      print *,'sag P0',sag
c      xx2= 1881.5
c      zz2=-3231.714

        xx2= 1882.049285187272
       zz2= -3229.56187376869



      call getsagittabin(sag,1,xx2,zz2)
      print *,'sag P1',sag
      
      if (debug.ge.2) then
         print *,'rfits in sag2d '
         do i=1,30
            print *,i,rfits(i)            
         enddo
         call shift(100.d0,ycham(1),zcham(1)+50.d0,xx,yy,zz)
         print *,'test BIL ',xx,yy,zz
         call shift(100.d0,ycham(2),zcham(2)+50.d0,xx,yy,zz)
         print *,'test BML ',xx,yy,zz
         call shift(100.d0,ycham(3),zcham(3)+50.d0,xx,yy,zz)
         print *,'test BOL ',xx,yy,zz
      endif
      end



      subroutine sag2dnew
C===========================================================
* Get sagitta as function of postion on BML plane.
* Also errors are calculated from MINOS information.
C===========================================================
      implicit none
      include 'mcommon'
      integer nx,nz,i,j
      real x2,z2,sag
      double precision xx2,zz2,yy2,xx,yy,zz
      double precision yya,yyb,yyc,yyd,epartn,
     + rbil(3),rbml(3),rbol(3),
     +  xx1,yy1,zz1, xx3,yy3,zz3
      real dxbin,dzbin,x1,z1
      nx=10
      nz=5

      call hbook2(92,'sag bml 1',nx,-real(xcham(2))/2.,
     +            real(xcham(2)/2.),nz,-1440./2.,1440./2.,0.)
      call hbook2(93,'sag bml 1',nx,-real(xcham(2))/2.,
     +            real(xcham(2)/2.),nz,-1440./2.,1440./2.,0.)


      call hijxy(92,1,2,x1,z1)
      call hijxy(92,2,2,x2,z2)
      dxbin=(x2-x1)
      dzbin=z2-z1
      do i=1,nx
         do j=1,nz
            yy2=ycham(2)
            call hijxy(92,i,j,x2,z2)
            x2=x2+dxbin/2.
            z2=z2+dzbin/2.
            xx2=0. + dble(x2)
            zz2=zcham(2) + dble(z2)
            yy1=ycham(1)
            yy2=ycham(2)
            yy3=ycham(3)
            xx1=xx2/ycham(2) * ycham(1)
            xx3=xx1+(xx2-xx1)/(ycham(2)-ycham(1))*
     +             (ycham(3)-ycham(1))
            zz1=zz2/ycham(2) * ycham(1)
            zz3=zz1+(zz2-zz1)/(ycham(2)-ycham(1))*
     +              (ycham(3)-ycham(1))
            rbil(1)=xx1
            rbil(2)=yy1
            rbil(3)=zz1
            rbml(1)=xx2
            rbml(2)=yy2
            rbml(3)=zz2
            rbol(1)=xx3
            rbol(2)=yy3
            rbol(3)=zz3
            call sagcal(rbil,rbml,rbol,sag)
            print *,'from sagcal ',x2,z2,sag
            call hfill(92,x2,z2,sag)
            call sagclas(rbil,rbml,rbol,sag)
            call hfill(93,x2,z2,sag)
         enddo
      enddo
      end



      subroutine getsagittabin(sag,ishift,xx2,zz2)
***************************************************************
* calculate sagitta. Baseline from point defined on BML xx2,zz2
*                                                             *
* input: ishift=0 use standard geometry.                      * 
*        ishift=1 use shifted=result-from-fit geometry        * 
*                                                             *
* output: sag=sagitta                                         *
***************************************************************
      implicit none
      include 'mcommon'

      
      real sag
      integer ishift,i

      double precision yy1,yy2,yy3,zz1,zz2,zz3,
     +                 ybil,ybml,ybol,zbil,zbml,zbol,
     +  xx1,xx2,xx3,xbil,xbml,xbol
      double precision dzdy,b,a,ap,bp,yl,zl,d,dy,dz,theta

      yy1=ycham(1)
      yy2=ycham(2)
      yy3=ycham(3)

      xx1=xx2/ycham(2) * ycham(1)
      xx3=xx1+(xx2-xx1)/(ycham(2)-ycham(1))*(ycham(3)-ycham(1))
      
c      xx3=xx2/xcham(2) * xcham(3)

      zz1=zz2/ycham(2) * ycham(1)
      zz3=zz1+(zz2-zz1)/(ycham(2)-ycham(1))*(ycham(3)-ycham(1))

c      zz3=zcham(3) + (zz2-zcham(2))/1440. * 2160.
      


      
      if (ishift.eq.0) then
         xbil=xx1
         xbml=xx2
         xbol=xx3
         ybil=yy1
         ybml=yy2
         ybol=yy3
         zbil=zz1
         zbml=zz2
         zbol=zz3
      else
         call shift(xx1,yy1,zz1,xbil,ybil,zbil)
         call shift(xx2,yy2,zz2,xbml,ybml,zbml)
         call shift(xx3,yy3,zz3,xbol,ybol,zbol)
      endif
      dzdy=(zbol-zbil)/(ybol-ybil)
      b=zbil-dzdy*ybil
      a=dzdy
      
C     closest approach z= a y + b
      if (a.ne.0.d0) then
         ap=-1.d0/a
      else
         ap=999999.d0
         print *,'WARININ getsag a=0 '
      endif
      bp=zbml-ap*ybml
      if (a-ap.ne.0.d0) then
         yl=(bp-b)/(a-ap)
      else
         yl=999999.d0
         print *,'WARININ getsag a-ap=0 '
      endif
      zl=a*yl+b
      if (abs(zl - (ap*yl+bp)).gt.0.1) 
     +     print *,'SHIT getsag ',i,zl,(ap*yl+bp)
      d=sqrt( (yl-ybml)**2 + (zl-zbml)**2 )
      if (zl.lt.zbml) d=-d
      sag=d

C TEST remove
c         dy=ybol-ybil
c         dz=zbol-zbil
c         theta=atan2(dy,dz)
c      sag=   cos(theta)*(ybml-yl)+
c     +        -sin(theta)*(zbml-zl)
c         sag=ybml

      if (debug.ge.2) then
         print *,'---- test tabin '
         print *,xbil,ybil,zbil
         print *,xbml,ybml,zbml
         print *,xbol,ybol,zbol
         print *,sag
      endif

      end

      subroutine fouten
C===============================================================
C Read MINOS information. Prepare for routine DPARTN
C===============================================================
      implicit none
      include 'mcommon'
      integer maxpar
      PARAMETER ( MAXPAR = 25 )
      COMMON/COPARA/AA(MAXPAR),DA(MAXPAR),CA(MAXPAR,MAXPAR),PSC(50)
      double precision aa,da,ca,psc
      CHARACTER*10 NAMIUT(100)
C
      INTEGER iut,i,j
      Double precision VALIUT(MAXPAR),ERRIUT(MAXPAR),
     +                 COVIUT(MAXPAR,MAXPAR),BO1,BO2
      integer IVI(MAXPAR)

      DO 620 IUT=1,MAXPAR
      CALL MNPOUT(IUT,NAMIUT(IUT),VALIUT(IUT),ERRIUT(IUT),BO1,BO2,
     +                   IVI(IUT))
      print *,'i ivi ',iut,ivi(iut)
 620  CONTINUE
      CALL MNEMAT(COVIUT,MAXPAR)
C
C--   Copy errors & covariance matrix for error calculation
      do i=1,maxpar
         da(i)=0.
         do j=1,maxpar
            ca(i,j)=0.
         enddo
      enddo
      DO 705 I = 1,MAXPAR
        IF(IVI(I).LE.0) GOTO 705
        DA(I) = ERRIUT(I)
        DO 700 J = 1,MAXPAR
          IF(IVI(J).LE.0) GOTO 700
          CA(I,J) = COVIUT(IVI(I),IVI(J))
 700  CONTINUE
 705  CONTINUE
      

      end
C     ==================================
      SUBROUTINE DPARTN(xx,zz,EPARTN)
C     ==================================
C--
C   Just standard error propagation:
C   Df**2 = Sum_i Sum_j (df/dpi)*(df/dpj) * cov(i,j)
C Calculates Errors on sagitta based on parameters in 'rfits'
C and error matrices prepared by 'fouten'.
C-- 
      implicit none
      include 'mcommon'
      integer maxpar
      PARAMETER ( MAXPAR = 25 )
      COMMON/COPARA/AA(MAXPAR),DA(MAXPAR),CA(MAXPAR,MAXPAR),PSC(50)
      double precision aa,da,ca,psc
      integer i,ip,jp
      double precision DER(MAXPAR),xx,zz,dmin,dmax,epartn,
     +                 dpar,fac
      real sag

      do i=1,maxpar
         der(i)=0.
      enddo
      DO 50 IP = 1,MAXPAR
        IF(CA(IP,IP).EQ.0.) GOTO 50
        DPAR    = SQRT(CA(IP,IP))
        rfits(IP)  = rfits(IP)+DPAR
        call getsagittabin(sag,1,xx,zz)
        DMAX    = dble(sag)
        rfits(IP)  = rfits(IP)-2.*DPAR
        call getsagittabin(sag,1,xx,zz)
        DMIN    = dble(sag)
        rfits(IP)  = rfits(IP)+DPAR
        DER(IP) = 0.5*(DMAX-DMIN)/DPAR
  50  CONTINUE

      EPARTN = 0.
      DO 100 IP = 1,MAXPAR
        IF(CA(IP,IP).EQ.0.) GOTO 100
        DO  70 JP = IP,MAXPAR
          IF(CA(IP,JP).EQ.0.) GOTO 70
          IF(JP.EQ.IP) FAC = 1.
          IF(JP.NE.IP) FAC = 2.
          EPARTN = EPARTN+FAC*DER(IP)*DER(JP)*CA(IP,JP)
  70    CONTINUE
 100  CONTINUE

      EPARTN = SQRT(EPARTN)

      RETURN
      END

















