        program inplane_rec                                  
*----------------------------------------------------------------------              
* 
*   Reconstruct MDT chamber deformation out of the RASNIK inplane informations                                                                              
*   General case for rectangular and trapezoidal chambers (8/08/97).                                                                               
* 
*  Coordinate system: 
*                 x along the  wires
*                 y perpendicular to the chamber plane
*                 z along CP axis
*  Origin = middle of central cross plate                                                                              
*  Cross plate numbering : 
*                     -1 (CPM,mask CP x<0),
*                      0 (CPL, lens CP x=0, central=reference)
*                     +1 (CPC, CCD CP  x>0)         
*  RASNIK line number: 1 bottom straight (z<0)                                  
*                      2 diagonal arriving at the top ccd                           
*                      3 diagonal arriving at the bottom ccd                        
*                      4 top straight (z>0)
*
*  INPUT:
*   
*    id_mdt     :  MDT identification number (to be used for geometry
*                  parameters)    
*    data_rasnik(1:4,iras) (beware of the signs) :
*                 dy (mm) : y Mask coordinate at the CCD center
*                 dz (mm) : z Mask coordinate at the CCD center
*                 dg/g    : departure from 1 of the image scale
*                 da(rad) : angular rotation of the mask wrt CCD
*
*  OUTPUT: 
*
*    deform_param(1:15) :
*
*           1:   Sum x translations of extreme crossplates (CPM,CPC)
*           2:   Sum y translations
*           3:   Sum z translations
*           4:   rotation wrt x axis of CPM 
*           5:   rotation wrt x axis of CPC
*           6:   rotation wrt y axis of CPM
*           7:   rotation wrt y axis of CPC
*           8:   thermal expansion parameter of CPM (CPL=ref) (0=no exp)
*           9:   thermal expansion parameter of CPC (CPL=ref)
*          10:   sagitta of CPM (wrt extreme masks, assuming parabola)
*          11:   sagitta of CPL (wrt extreme lenses, assuming parabola)
*          12:   sagitta of CPC (wrt extreme CCDs, assuming parabola)
*          13:   dont use
*          14:   dont use
*          15:   sum of CPM and CPC rotations arount their longit. axis
*
*     For rectangular chambers only the first 12 parameters are used
*
*-----------------------------------------------------------------------
      dimension  data_rasnik(4,4), deform_param(15)
*
      parameter (nras=4)                                                        
      common /mdt_geo/ xlcp, zl14, dz12, dz34, zlcp, alfa                                      
     +               , zras(-1:1,nras), xras(-1:1,nras)                        
*               
      common /rasnik_meas/ dym(nras), dzm(nras), dgm(nras), dam(nras)               
      common /rec_inpl/ dtxf, dtyf, dtzf                                             
     +                , drxzf(-1:1), dryf(-1:1)                                    
     +                , dtpf(-1:1), dsgf(-1:1)
     +                , ddxf, ddzf, drzxf
     +                , chi21, chi22
      logical debug                                                                                          
*----------------------------------------------------------------------- 
      data idold / -999999/  
*
*                                                                               
*  Define chamber geometry and RASNIK position and performances 
*
      id_mdt=48
      
      if(id_mdt.ne.idold)then
*
        call inplane_geometry(id_mdt)  !  retieve inplane geometry parameters
*
        call choose_parameters    !   choose the deformation paramater you
*                                     want to fit and the RASNIK informations
*                                     you want to use
*
        call inplane_init(id_mdt)
*
        idold = id_mdt
      endif
*
*   data conversion:
*
      do ir = 1,4
*      factor = -xras(1,ir)/xras(-1,ir)
*       dym(ir) = data_rasnik(1,ir)*factor                   
*       dzm(ir) = data_rasnik(2,ir)*factor                   
*       dgm(ir) = data_rasnik(3,ir)                   
*       dam(ir) = data_rasnik(4,ir)
        read *,dym(ir),dzm(ir),dgm(ir),dam(ir)
      enddo
*                   
*   reconstruct chamber deformation parameters 
*
      debug = .false.   !  for  printout
      call rec_inplane(debug)
*
      call ucopy(dtxf,deform_param(1),4)
      call ucopy(drxzf(1),deform_param(5),2)
      call ucopy(dryf(1),deform_param(7),2)
      call ucopy(dtpf(1),deform_param(9),7)
      chi2 = chi21+chi22
*
      print *,'chi2 ', chi2
      do ii=1,16
	print *, 'deformers ',deform_param(ii)
       enddo
      return
      end
**************************************************************************                                                                       
      subroutine inplane_geometry(id_mdt)                                                                            
*------------------------------------------------------------------------- 
*                                                                               
*   Input inplane geometry and RASNIK resolutions:                                                             
*                                                                               
*  Inplane geometry                                     
      parameter (nras=4)                                                        
      common /mdt_geo/ xlcp, zl14, dz12, dz34, zlcp, alfa                                      
     +               , zras(-1:1,nras), xras(-1:1,nras)                        
*                                                                               
*  Resolutions on CCD                                                       
      common /rasnik_res/ sym(nras), szm(nras), sgm(nras), sam(nras)
*
*------------------------------------------------------------------------- 
*
*         Here DATCHA BML as an example
*
      zlcp = 2100.  !  Cross plate length (mm) ( along z axis)                                   
      xlcp = 5000.  !  Distance between end crossplates (mm) at z = 0                     
      zl14 = 1070.  !  distance in z between straight RASNIKs (1 and 4)
      dz12 = 270.   !  distance in z between mask 1 and mask 2                          
      dz34 = 270.   !  distance in z between mask 1 and mask 2                               
       
cc      alfa = 14.  ! angle (degree) of CP w.r.t. z axis (smaller side at low z)                      
      alfa = 0.     !  for barrel chambers
* 
*   RASNIK resolutions                                                            
*                                                                               
      do ir = 1,nras                                                            
        sym(ir) = 0.001    !  mm  along y                                      
        szm(ir) = 0.001    !  mm        z                                      
        sgm(ir) = 5.0 e-5  !  on image scale                                   
        sam(ir) = 0.000050 ! rad  on image rotation                           
      enddo                                                                     
*
      return
      end                 
**************************************************************************** 
      subroutine choose_parameters
*------------------------------------------------------------------------- 
*                                                                               
*  Only the parameters with idfit.ne.0 and the measurements with idmeas.ne.0
*  are used in the the fit
*
      parameter (nras=4)                                                        
      parameter (npfitm=15)
      common /rec_fit/ idfit(npfitm), idmeas(4*nras)
     +               , npfit(2), ipfit(npfitm,2)
* 
*------------------------------------------------------------------------- 
*                                                                               
*     Nb of fitted parameters for each fit
*
       alfa=0
      npfit(1) = 7
      if(alfa.eq.0.)npfit(1)=6
      npfit(2) = 8
      if(alfa.eq.0.)npfit(2)=6
*
      npf = npfit(1)+npfit(2)   !  Number of a priori free parameters
      do ii=1,npf
       idfit(ii) = 1
      enddo
      do ii=npf+1,npfitm
       idfit(ii) = 0
      enddo
      do ii=1,4*nras
       idmeas(ii) = 1
      enddo
*
*   If you wish to drop the free parameter nb i out of the nominal 16 ones
*   do it here by putting idfit(i) =0 
*
c           idfit(1) = 0     !  Sum x trans
c           idfit(2) = 0     !  Sum y trans
c           idfit(3) = 0     !  Sum z trans
c           idfit(4) = 0     !  rot/xz  CPM
c           idfit(5) = 0     !  rot/xz  CPC
c           idfit(6) = 0     !  rot/y  CPM
c           idfit(7) = 0     !  rot/y  CPC
c           idfit(8) = 0     !  expansion CPM
c           idfit(9) = 0     !  expansion CPC
c           idfit(10) = 0    !  sag CPM
c           idfit(11) = 0    !  sag CPL
c           idfit(12) = 0    !  sag CPC
           idfit(13) = 0    !  diff x trans
           idfit(14) = 0    !  diff z trans
c           idfit(15) = 0    !  sum zx rot
*
*   If you wish to drop  RASNIK measurement nb i out of the 4*nras  
*   do it here by putting idmeas(i)=0 
*
       do ir=1,nras
c         idmeas( ir) = 0          !   dy meas
c         idmeas(nras + ir) = 0    !   dz meas
c         idmeas(2*nras + ir) = 0  !   scale meas
c         idmeas(3*nras + ir) = 0  !   angle meas
       enddo
*
*
       print *,'  Nb of parameters: ', npf
       do ix=1,npf
        if(idfit(ix).eq.0)then
          print *,' Parameter nb',ix,'  ignored in the fit'
        endif
       enddo                      
       do im=1,4*nras
        if(idmeas(im).eq.0)then
          print *,' Measurement nb',im,'  ignored in the fit'
        endif
       enddo
* 
      return
      end                 
**************************************************************************** 
      subroutine inplane_init(id_mdt)                                              
* ------------------------------------------------------------------------                                                                              
*  Positions of inplane RASNIK sensors 
*  Used parameters and measurements
*  Calculate the matrix coefficients for the first order approx .                                           
*-------------------------------------------------------------------------                                                                              
*                                                                               
*  Coordinates of RASNIKs sensors (cross plate nb, line nb)                     
      parameter (nras=4)                                                        
      common /mdt_geo/ xlcp, zl14, dz12, dz34, zlcp, alfa                                      
     +               , zras(-1:1,nras), xras(-1:1,nras)                        
*
*   Coefficient of the relation between rasnik readout dras and chamber
*   deformation parameters dpar : dras = sum (cya or czg * dpar)
*              
      common /coeff_inpl/ cya(2*nras,8), czg(2*nras,8)
*
      parameter (npfitm=15)
      common /rec_fit/ idfit(npfitm), idmeas(4*nras)
     +               , npfit(2), ipfit(npfitm,2)
*-------------------------------------------------------------------------  
*                                                                               
      pi =  4.*atan(1.)
      alfar = alfa*pi/180.
      tana = tan(alfar)
      cosa = cos(alfar)
      sina = sin(alfar)                                     
*
*    Assume symmetry w.r.t. x axis                                              
*    Middle cross plate exactly at the middle                                   
*                                                                               
*   Calculate nominal sensor coordinates                                                
* 
*  Masks:                                                                              
      zras(-1,1) = -zl14/2.                                                   
      zras(-1,4) =  zl14/2.                                                    
      zras(-1,2) = zras(-1,1) + dz12                                            
      zras(-1,3) = zras(-1,4) - dz34
      do ir=1,nras 
       xras(-1,ir) = -xlcp/2. - zras(-1,ir)*tana
      enddo
*
*  CCD                                                                              
      zras(1,1) = zras(-1,1)                                                    
      zras(1,4) = zras(-1,4)                                                    
      zras(1,2) = zras(1,4)                                                     
      zras(1,3) = zras(1,1)
      do ir=1,nras 
       xras(1,ir) = xlcp/2. + zras(1,ir)*tana
      enddo
*                                                                               
*  Lens position: lens assumed to be on the central crossplate  
*                                                           
      do ir = 1,nras 
       xras(0,ir) = 0.                                                          
       zras(0,ir) = zras(-1,ir) - xras(-1,ir)
     +           * (zras(-1,ir) - zras(1,ir))/(xras(-1,ir) - xras(1,ir))                             
      enddo
*      
*------------------------------------------------------------------------------
*
*   Fitted parameter indices:
*
        ipfit(1,1) = 2     !  First fit
        ipfit(2,1) = 4
        ipfit(3,1) = 5
        ipfit(4,1) = 10
        ipfit(5,1) = 11
        ipfit(6,1) = 12
        ipfit(7,1) = 15
* 
        ipfit(1,2) = 1      ! second fit
        ipfit(2,2) = 3
        ipfit(3,2) = 6
        ipfit(4,2) = 7
        ipfit(5,2) = 8
        ipfit(6,2) = 9                    
        ipfit(7,2) = 13
        ipfit(8,2) = 14
*
*  Calculate the coefficients of the 1st order relation between rasnik values
*   and deformation parameters
*
      call vzero(cya,2*nras*8)
      call vzero(czg,2*nras*8)
*
      do ir = 1,nras
*                                                  
       nr = 2*(ir-1)                                                             
       fmc = xras(-1,ir)/xras(1,ir)     
       fs = 0.5*(1.-fmc)                                             
       fd = 0.5*(1.+fmc)
       tanr = (zras(-1,ir)-zras(0,ir))/xras(-1,ir)
* 
*  (dym,dam) = cya * ( dy, drxz(-1), drxz(1), dsg(-1,0,1), drzx )
*
       cya(nr+1,1) = -fs                                                       
       cya(nr+1,2) =  zras(-1,ir)                                               
       cya(nr+1,3) = -zras( 1,ir)*fmc
       cya(nr+1,4) = -(1.- 4.*(zras(-1,ir)/zl14)**2)                                            
       cya(nr+1,5) =  (1.- 4.*(zras( 0,ir)/zl14)**2)*fs*2.                                                   
       cya(nr+1,6) =  (1.- 4.*(zras( 1,ir)/zl14)**2)*fmc 
       cya(nr+1,7) = 0.
*                                                       
       cya(nr+2,1) = 0.                                                        
       cya(nr+2,2) = -cosa**2                                                        
       cya(nr+2,3) =  cosa**2                                                  
       cya(nr+2,4) =  8.*zras(-1,ir)/zl14**2                                                       
       cya(nr+2,5) =  0.                                                        
       cya(nr+2,6) = -8.*zras( 1,ir)/zl14**2                                                       
       cya(nr+2,7) =  cosa*sina                                                        
*	print *,'de cya check '
*	do ii=1,8
*	print *,'nr 1 ',ii,cya(nr+1,ii)
*	print *,'nr 2 ',ii,cya(nr+2,ii)
*	enddo
* 
* (dzm, dgm) = czg * ( dx, dz, dry(-1), dry(1), dtp(-1), dtp(1),ddx,ddz)
*
       czg(nr+1,1) =  fs*tanr
       czg(nr+1,2) = -fs
       czg(nr+1,3) =  zras(-1,ir) * (tanr-tana)          
       czg(nr+1,4) = -zras( 1,ir) * (tanr+tana) * fmc                     
       czg(nr+1,5) = -zras(-1,ir) * (1.+tana)                                               
       czg(nr+1,6) =  zras( 1,ir) * (1.-tana) * fmc                                              
       czg(nr+1,7) =  fd*tanr                    
       czg(nr+1,8) = -fd
*       
       czg(nr+2,1) = -fs/xras(-1,ir)
       czg(nr+2,2) = 0.                                                         
       czg(nr+2,3) = -zras(-1,ir)/xras(-1,ir)                                    
       czg(nr+2,4) =  zras( 1,ir)/xras( 1,ir)                       
       czg(nr+2,5) =  zras(-1,ir)/xras(-1,ir) * tana
       czg(nr+2,6) =  zras( 1,ir)/xras( 1,ir) * tana
       czg(nr+2,7) = -fd/xras(-1,ir)
       czg(nr+2,8) = 0.
*                                                         
      enddo 
*                                                                               
      return
      end
*************************************************************************                                                                   
      subroutine rec_inplane(debug)
*------------------------------------------------------------------------- 
*   Reconstruct MDT chamber deformation parameters 
*-------------------------------------------------------------------------
*                                                                               
      parameter (nras=4)
*
*  16 RASNIK measurements (direct CCD outputs):                                                             
*  1/4)   dym(ir)   : y measured deviations on the CCD                            
*  5/8)   dzm(ir)   : z measured deviations on the CCD                            
*  9/12)  dgm(ir)   : departure from 1 of the image size scale                        
*  13/16) dam(ir)   : image rotation on ccd (almost all identical)
*               
      common /rasnik_meas/ dym(nras), dzm(nras), dgm(nras), dam(nras)               
*
* Only the 15 following deformation parameters are reconstructed by the
* in plane alignment system:
*      1) dtxf = dtx(-1)+dtx(1)   Sum of extreme CP translations/x                         
*      2) dtyf = dty(-1)+dty(1)   Sum of extreme CP translations/y                         
*      3) dtzf = dtz(-1)+dtz(1)   Sum of extreme CP translations/z 
*    4,5) drxzf(-1), drxzf(1)  =  rot/ x axis
*                     +- tan(alfa)( rot/z axis + (dty(-1)-dty(1))/(xlcp/2.)
*    6,7) dryf(-1), dryf(1)       rot/ y axis
*    8,9) dtpf(-1), dtpf(1)  expansion coef of extreme CP plates 
*  10,11,12), dsgf(-1),dsgf(0),dsgf(1) sagittas of the cross-plates
*     13) ddxf = dtx(-1)-dtx(1)   Diff of extreme CP translations/x
*     14) ddzf = dtz(-1)-dtz(1)   Diff of extreme CP translations/z
*     15) drzxf = sum (rot/z axis) -tan(alfa)*(diff rot/x axis)
*                 - 2*(dty(-1)-dty(1))/(xlcp/2.) 
* 
*  For rectangular chamber, only the first 12 parameters matter
*                    
      common /rec_inpl/ dtxf, dtyf, dtzf                                             
     +                , drxzf(-1:1), dryf(-1:1)                                    
     +                , dtpf(-1:1), dsgf(-1:1)
     +                , ddxf, ddzf, drzxf
     +                , chi21, chi22
*
*  Only the parameters with idfit.ne.0 and the measurements with idmeas.ne.0
*  are used in the the fit
*
      parameter (npfitm=15)
      common /rec_fit/ idfit(npfitm), idmeas(4*nras)
     +               , npfit(2), ipfit(npfitm,2)
*
*  Coefficient of the relation between these measurement and the deformation
*  parameters:
      common /coeff_inpl/ cya(2*nras,8), czg(2*nras,8)
*                                                                               
*   RASNIK resolutions:                                                        
      common /rasnik_res/ sym(nras), szm(nras), sgm(nras), sam(nras)
*        
*------------------------------------------------------------------------------
      dimension vameas(4*nras), sgmeas(4*nras)
      dimension crr(2*nras,8)
      dimension ipmeas(4*nras)
      dimension dpfit(npfitm)                                        
      dimension varm(20),erm(20), parx(20), coeff(20,20)
      logical debug
*------------------------------------------------------------------------------
*
      call ucopy(dym(1), vameas,4*nras)
      call ucopy(sym(1), sgmeas,4*nras)
      call vzero(dpfit,npfitm)
      call vzero(dtxf,20)
*
*                                                                               
*  Now solve 2 linear systems (linear approx):                                         
*
      do ifit = 1,2
*
      if(ifit.eq.1)then
*                                                                               
*  1) dtyf, drxz(-1), drxz(1), dsgf(-1:1), drzx  from dym and dam
*
*   Measured quantities index:
        do ir=1,nras                                                           
         nr=2*(ir-1)                                                                
         ipmeas(nr+1) = ir                   
         ipmeas(nr+2) = 3*nras + ir 
        enddo 
        call ucopy(cya(1,1),crr(1,1),16*nras)                  
*
      elseif(ifit.eq.2)then
*                                                                              
*  2) dtxf, dtzf, dry(-1), dry(1), dtp(-1), dtp(1),ddx,ddz from dzm and dgm               
*                                                                              
*   Measured quantities index:
        do ir=1,nras                                                           
         nr=2*(ir-1)                                                                     
         ipmeas(nr+1) = nras + ir                   
         ipmeas(nr+2) = 2*nras + ir 
        enddo
        call ucopy(czg(1,1),crr(1,1), 16*nras)                  
*                   
      endif
*                                                                   
      nbm=0                                                                     
*      print *,'ifit npfit ',ifit, npfit(ifit)
      do i1=1,2*nras                                                               
*            print *,'i1 ipmeas idmeas ',i1-1,ipmeas(i1),
*     +               idmeas(ipmeas(i1))
       if(idmeas(ipmeas(i1)).ne.0)then
         nbm=nbm+1
         varm(nbm) = vameas(ipmeas(i1))
         erm(nbm)  = sgmeas(ipmeas(i1))
         nbx = 0
         do i2=1,npfit(ifit)                                                               
*      print *,'i2 idfit ',i2-1, idfit(ipfit(i2,ifit))
          if(idfit(ipfit(i2,ifit)).ne.0)then
            nbx = nbx+1                                          
            coeff(nbm,nbx)=crr(i1,i2)
            print *,'coefs',i1,i2,crr(i1,i2)
          endif                                                  
         enddo
       endif                                                                     
      enddo
* 
      if(nbm.ge.nbx.and.nbx.gt.0)then
*
        if(debug)then
         print *,' varm=',(varm(k),k=1,nbm)                                        
         print *,'  erm=',(erm(k),k=1,nbm)                                        
         do l=1,nbx                                                                
*          print *,' coeff=',(coeff(k,l),k=1,nbm)                                    
         enddo                                                                     
        endif                                                                     
*                                                                               
        call solvek(nbm,nbx,varm,erm,coeff,parx,chi2)                             
* 
*         print *,' after solvek chi2 =',chi2                                       
*
        if(ifit.eq.1)then
          chi21 = chi2
        else
          chi22 = chi2
        endif 
        nbx = 0
        do i2=1,npfit(ifit)
         if(idfit(ipfit(i2,ifit)).ne.0)then
           nbx = nbx+1                                                                       
           dpfit(ipfit(i2,ifit)) = parx(nbx)
*         print *,' parx =',parx(nbx)                               
         endif
        enddo
*
      endif
*
      enddo
*
      call ucopy(dpfit(1), dtxf, 4)                                                                    
      call ucopy(dpfit(5), drxzf(1), 2)                                            
      call ucopy(dpfit(7), dryf(1), 2)                                            
      call ucopy(dpfit(9), dtpf(1), 7)                                                                        
*                                                                               
      return
      end                                                          
*************************************************************************                                                                   
      subroutine solvek(nbm,nbx,vm,erm,cvx,x,chi2)                              
*                                                                               
*   Solve for the nbx unknown parameters x using a chi2 minimization            
*   performed  the nbm measured variables vm (with error erm)                   
*   expressed as a linear combination of x with coeff cvx(nbm,nbx)              
*                                                                               
      dimension vm(20), erm(20), x(20), cvx(20,20)                              
      dimension a(20,20), b(20), wr(40)                                         
*                                                                               
      do k=1,nbx                                                                
       b(k)=0.                                                                  
       do j=1,nbx                                                               
        a(j,k)=0.                                                               
        do i=1,nbm                                                              
         a(j,k)=a(j,k)+cvx(i,j)*cvx(i,k)/erm(i)**2                              
        enddo                                                                   
       enddo                                                                    
       do i=1,nbm                                                               
         b(k)=b(k)+vm(i)*cvx(i,k)/erm(i)**2                                     
       enddo                                                                    
      enddo                                                                     
*                                                                               
*   solve the linear system                                                     
*                                                                               
      call reqn(nbx,a,20,wr,ifail,1,b)                                          
*                                                                               
      call ucopy(b,x,nbx)                                                       
*                                                                               
* final chi2                                                                    
*                                                                               
      chi2=0.                                                                   
      do i=1,nbm                                                                
       vt=0.                                                                    
       do j=1,nbx                                                               
        vt=vt+cvx(i,j)*x(j)                                                     
       enddo                                                                    
       chi2=chi2+((vm(i)-vt)/erm(i))**2                                         
      enddo                                                                     
*                                                                               
      return                                                                    
      end                                                                       

