C-----------------------------------------------------------------------
C  Routine to align one circle by accumulating a distribution
C   of intensity values against degrees & then
C   finding the median of the distribution.
C
C  Modifications: Mark Koennecke, April 2000
C  Added code for doing PH optimizations as well.
C  Added code for monitoring the centering process as well.
C  When a peak is not found, drive back to start and give an FP error 
C  code instead of an FF. Then the alignement of another circle
C  might resolve the issue.
C-----------------------------------------------------------------------
      SUBROUTINE CENTRE (DX,ANG,ISLIT)
      INCLUDE 'COMDIF'
      DIMENSION XA(100),YA(100),AN(4),ST(4),ANG(4)
      CHARACTER ANGLE(4)*6
      DATA ANGLE/'2theta','Omega','Chi','PH'/
      INTEGER IRUPT
C
      external range    ! Prevent use of intrinsic function under GNU G77
C
      NATT = 0
C------- a debug flag! Set to 0 for no debug output
      IDEBUG = 1
C-----------------------------------------------------------------------
C  If CAD-4 call the scan fitting version of the routine
C-----------------------------------------------------------------------
      IF (DFMODL .EQ. 'CAD4') THEN
        CALL CADCEN (ISLIT)
        NATT = 0
        CALL ANGSET (THETA,OMEGA,CHI,PHI,NATT,IERR)
        ANG(1) = THETA
        ANG(2) = OMEGA
        ANG(3) = CHI
        ANG(4) = PHI
        RETURN
      ENDIF
C-----------------------------------------------------------------------
C  Get the starting values for the angles & set up the working ranges
C-----------------------------------------------------------------------
  100 CALL ANGET (ST(1),ST(2),ST(3),ST(4))
      IF (KI .EQ. 'ST') N = 1
      IF (KI .EQ. 'SO') N = 2
      IF (KI .EQ. 'SC') N = 3
      IF (KI .EQ. 'SP') N = 4
      ICHI = 0
      IF (ST(3) .GE. 350.0 .OR. ST(3) .LE. 10.0) ICHI = 1
      IPHI = 0
      IF (ST(4) .GE. 350.0 .OR. ST(4) .LE. 10.0) IPHI = 1
      IA = 0
      ISTEP = -1
      I = 50
      MAX = -1000000
      MIN =  1000000
C-----------------------------------------------------------------------
C  Step forwards or backwards on the appropriate circle, count & store
C-----------------------------------------------------------------------
  110 D1 = DX*(I - 50)
      IF (N .EQ. 1) D2 = -0.5*D1
      IF (N .EQ. 2) D2 =  0.5*D1
      DO 120 J = 1,4
        AN(J) = ST(J)
  120 CONTINUE
      AN(N) = AN(N) + D1
      CALL MOD360 (AN(N))
      IF (N .EQ. 1) THEN
        AN(2) = AN(2) + D2
        CALL MOD360 (AN(2))
      ENDIF
      IF (N .EQ. 2) THEN
        AN(1) = AN(1) + D2
        CALL MOD360 (AN(1))
      ENDIF
      IF (N .EQ. 3) AN(4) = ST(4)
      CALL ANGSET (AN(1),AN(2),AN(3),AN(4),IA,IC)
      IF (IC .NE. 0) THEN
        WRITE (COUT,10000)
        CALL GWRITE (ITP,' ')
        KI = 'FF'
        RETURN
      ENDIF
      CALL CCTIME (PRESET,COUNT)
      IF(IDEBUG .EQ. 1)THEN
        WRITE(COUT,20000),AN(1),AN(2),AN(3),AN(4),COUNT
20000   FORMAT('TH = ',F8.2,' OM = ',F8.2,' CH = ',F8.2,' PH = ', F8.2,
     &        ' CTS = ', F8.2)
      ENDIF 
      CALL KORQ(IRUPT)
      IF(IRUPT .NE. 1) THEN
        WRITE (COUT,10000)
        CALL GWRITE (ITP,' ')
        KI = 'FF'
        RETURN
      ENDIF
      CALL ANGET (AN(1),AN(2),AN(3),AN(4))
      CALL RANGE (ICHI,IPHI,AN)
      XA(I) = AN(N)
      IF (COUNT .LT. 1) THEN
        YA(I) = 0
        GO TO 110
      ENDIF
      YA(I) = COUNT
      IF (COUNT .GT. MAX) THEN
        MAX = COUNT
        IMAX = I
      ENDIF
      IF (COUNT .LT. MIN) MIN = COUNT
      IF (COUNT .GT. AFRAC*MAX) THEN
        I = I + ISTEP
        IF (I .GE. 1 .AND. I .LE. 100) GO TO 110
      ENDIF
C-----------------------------------------------------------------------
C  Sort out what happened on the low angle side (ISTEP = -1)
C
C  There are 4 situations to take care of on the low angle side.
C  1. The peak is at the low angle extremity.  Move and try again.
C  2. There is no significant low angle peak.
C  3. The peak behaves normally and the peak straddles the centre of
C     the search range, i.e. I = 50 is in the peak.
C  1. The peak is at the low angle extremity.  Move and try again.
C-----------------------------------------------------------------------
      IF (ISTEP .EQ. -1) THEN
        ILOW = I
C-----------------------------------------------------------------------
C  Case 1. Peak is at the low angle extremity.  Start again.
C-----------------------------------------------------------------------
        IF (IMAX .EQ. 1 .AND. AFRAC*MAX .GT. MIN) GO TO 100
C-----------------------------------------------------------------------
C  Cases 2 and 3.  No significant peak or normal peak.
C                  In either case do the high angle side.
C-----------------------------------------------------------------------
        IF (I .LT. 1 .OR. YA(50) .GE. AFRAC*MAX) THEN
          ISTEP = 1
          NBOT = I
          I = 51
          GO TO 110
        ENDIF
C-----------------------------------------------------------------------
C  Case 4. Peak is all on low angle side.
C          ILOW is where the peak ended, find where it started.
C-----------------------------------------------------------------------
        IF (YA(50) .LT. AFRAC*MAX) THEN
          DO 130 ISRCH = 1,50
            JSRCH = 51-ISRCH
            IF (YA(JSRCH) .GT. AFRAC*MAX) THEN
              NTOP = JSRCH
              NBOT = ILOW
              GO TO 200
            ENDIF
  130     CONTINUE
          NTOP = JSRCH
          NBOT = ILOW
          GO TO 200
        ENDIF
      ENDIF
C-----------------------------------------------------------------------
C  Sort out what happened on the high angle side.  (ISTEP = 1)
C
C  Again there are 4 cases to take care of
C  1.  The peak is at the high angle extremity.  Move and try again.
C  2.  There is no significant high angle peak.  This can only occur
C      after case 1 on the low side, i.e. there is no peak at all.
C  3.  It is a normal peak continuing from the low angle case 2.
C  4.  Peak is all on the high angle side.
C      IHIGH is where the peak ended, find where it started.
C-----------------------------------------------------------------------
      IF (ISTEP .EQ. 1) THEN
        IHIGH = I
C-----------------------------------------------------------------------
C  Case 1. Peak is at the high angle extremity.  Start again.
C-----------------------------------------------------------------------
        IF (IMAX .EQ. 100 .AND. AFRAC*MAX .GT. MIN) GO TO 100
C-----------------------------------------------------------------------
C  Case 2. There is no significant peak.
C
C  Modified: Drive back to start positions. So that other circle centering
C  will not fail.
C  Modified error code to give an FP in order to decide between 
C  interrupt and bad peak.
C  Mark Koennecke, April 2000
C-----------------------------------------------------------------------
        IF (ILOW .LT. 1 .OR. IHIGH .GT. 100) THEN
          WRITE (COUT,11000) ANGLE(N),ILOW,IHIGH
          CALL GWRITE (ITP,' ')
          KI = 'FP'
          CALL ANGSET(ST(1),ST(2),ST(3),ST(4),IA,IC)
          RETURN
        ENDIF
C-----------------------------------------------------------------------
C  Case 3.  Normal peak.
C-----------------------------------------------------------------------
        IF (YA(50) .GE. AFRAC*MAX) THEN
          NTOP = I - 1
C-----------------------------------------------------------------------
C  Case 4.  Peak is all on the high angle side.
C-----------------------------------------------------------------------
        ELSE
          DO 140 ISRCH = 50,100
            IF (YA(ISRCH) .GT. AFRAC*MAX) THEN
              NTOP = IHIGH - 1
              NBOT = ISRCH - 1
              GO TO 200
            ENDIF
  140     CONTINUE
          NTOP = IHIGH - 1
          NBOT = ISRCH - 1
        ENDIF
      ENDIF
C-----------------------------------------------------------------------
C  Find the median of the distribution
C-----------------------------------------------------------------------
  200 AREA = 0.0
      IF (NBOT .LT. 1 .OR. NTOP .GT. 100 .OR. MAX .LE. 25) THEN
      WRITE (COUT,11000) ANGLE(N),NBOT,NTOP,MAX
        CALL GWRITE (ITP,' ')
        KI = 'FF'
        RETURN
      ENDIF
      DO 210 I = NBOT,NTOP
        AREA = AREA + (XA(I+1) - XA(I))*(YA(I+1) + YA(I))*0.25
  210 CONTINUE
      S = 0.0
      DO 220 I = NBOT,NTOP
        S = S + (XA(I+1) - XA(I))*(YA(I+1) + YA(I))*0.5
        IF (S .GT. AREA) GO TO 230
  220 CONTINUE
  230 S = S - 0.5*(XA(I+1) - XA(I))*(YA(I+1) + YA(I))
C-----------------------------------------------------------------------
C  The centre is now in the Ith strip
C-----------------------------------------------------------------------
      DA = AREA - S
C-----------------------------------------------------------------------
C  Get the slope of the Ith strip & solve for X at AREA/2
C-----------------------------------------------------------------------
      IF (YA(I+1) .EQ. YA(I)) THEN
        XCENT = XA(I) + DA/YA(I)
      ELSE
        S = (YA(I+1) - YA(I))/(XA(I+1) - XA(I))
        IF ((YA(I)*YA(I) + 2.0*S*DA) .LE. 0) THEN
          WRITE (COUT,12000)
     $           NBOT,NTOP,I,(XA(III),YA(III),III = NBOT,NTOP)
          CALL GWRITE (ITP,' ')
          KI = 'FF'
          RETURN
        ENDIF
        DISC = SQRT(YA(I)*YA(I) + 2.0*S*DA)
        XCENT = XA(I) + (DISC - YA(I))/S
      ENDIF
C-----------------------------------------------------------------------
C  Put the answer in the correct range
C  N = 1 2THETA;   N = 2 Omega;   N = 3 Chi;   N = 4 Phi.
C-----------------------------------------------------------------------
      IF (N .EQ. 1) THEN
        DA = XCENT - ST(1)
        ST(1) = XCENT
        ST(2) = ST(2) - 0.5*DA
        CALL MOD360 (ST(2))
        ANG(2) = ST(2)
C-----------------------------------------------------------------------
C  OMEGA range
C-----------------------------------------------------------------------
      ELSE IF (N .EQ. 2) THEN
        DA = ST(2) + 180
        IF (DA .GE. 360) DA = DA - 360
        DA = XCENT - DA
        ST(1) = ST(1) + 0.5*DA
        CALL MOD360 (ST(1))
        XCENT = XCENT - 180.0
        CALL MOD360 (XCENT)
        ST(N) = XCENT
C-----------------------------------------------------------------------
C  CHI range
C-----------------------------------------------------------------------
      ELSE IF (N .EQ. 3) THEN
        XCENT = XCENT - ICHI*180.0
        CALL MOD360 (XCENT)
        ST(N) = XCENT
C-----------------------------------------------------------------------
C  PHI range
C-----------------------------------------------------------------------
      ELSE IF (N .EQ. 4) THEN
        XCENT = XCENT - IPHI*180.0
        CALL MOD360 (XCENT)
        ST(N) = XCENT
      ENDIF
C-----------------------------------------------------------------------
C  Set angles to the max values
C-----------------------------------------------------------------------
      CALL ANGSET (ST(1),ST(2),ST(3),ST(4),IA,IC)
      ANG(N) = ST(N)
      IF (N .NE. 4) ANG(4) = ST(4)
      KI = ' '
      RETURN
10000 FORMAT (' Real Collision in routine CENTRE')
11000 FORMAT (' Alignment Failure on ',A,'.  NBOT, NTOP',2I4,' MAX',I6)
12000 FORMAT (3I6,/,(10F10.4))
      END
C
C-----------------------------------------------------------------------
C  Subroutine to do a fine (1) or coarse (0) centreing on a specified 
C  circle for the CAD4 using the routine GENSCN.
C  Different for the 2theta circle.
C-----------------------------------------------------------------------
      SUBROUTINE CADCEN (ISLIT)
      INCLUDE 'COMDIF'
      ICPSMX = 25000
C-----------------------------------------------------------------------
C  Set the attenuator if necessary
C-----------------------------------------------------------------------
      TIME = 1.0
      CALL CCTIME (TIME,COUNT)
      IF (COUNT .GT. ICPSMX .AND. NATT .EQ. 0) THEN
        NATT = 1
        COUNT = COUNT/ATTEN(2)
        CALL ANGSET (THETA,OMEGA,CHI,PHI,NATT,IERR)
      ENDIF
C-----------------------------------------------------------------------
C  2Theta circle (and chi)
C  After the initial omega/2theta scan, this completes the alignment 
C  for a CAD-4 machine
C    1. Scans with the + 45deg slit;
C    2. Scans with the - 45deg slit;
C    3. Works out the 2theta & chi corrections via EULKAP.
C-----------------------------------------------------------------------
C      write (lpt,99999) ki,theta,omega,chi,phi
C99999 format (' Before ',a,2x,4f8.3)
      IF (KI .EQ. 'ST') THEN
        SSPEED = 10
        IF (COUNT .LT. 2000) SSPEED = 5.0
        IF (COUNT .LT. 1000) SSPEED = 2.5
        IF (COUNT .LT.  400) SSPEED = 1.0
        ICIRCLE = 1
        NPTS = 50
        WIDTH = 2.5
        STEP = WIDTH/NPTS
        ISLIT = 3
        CALL GENSCN (ICIRCLE,SSPEED,STEP,NPTS,ISLIT,ICOL)
        CALL PFIT (NPTS,BEST)
        ISLIT = 0
        IF (KI .EQ. 'FF') GO TO 200
        FIRST = BEST*STEP
        CALL ANGSET (THETA,OMEGA,CHI,PHI,NATT,IERR)
        ISLIT = 4
        CALL GENSCN (ICIRCLE,SSPEED,STEP,NPTS,ISLIT,ICOL)
        CALL PFIT (NPTS,BEST)
        ISLIT = 0
        IF (KI .EQ. 'FF') GO TO 200
        SECOND = STEP*BEST
        DELTAC = 0.5*(FIRST - SECOND)/(2.0*SIN(0.5*THETA/DEG))
        CHI = CHI - DELTAC
        ISLIT = 0
        IF (ABS(DELTAC) .GE. 1.0) ISLIT = 1
        THETA = THETA + 0.5*(FIRST + SECOND)
        OMEGA = OMEGA - 0.25*(FIRST + SECOND)
      ENDIF
C-----------------------------------------------------------------------
C  Omega circle
C-----------------------------------------------------------------------
      IF (KI .EQ. 'SO') THEN
        ICIRCLE = 2
        NPTS = 50
        STEP = 1.0/NPTS
        SSPEED = 5
  120   CALL GENSCN (ICIRCLE,SSPEED,STEP,NPTS,ISLIT,ICOL)
        CALL PFIT (NPTS,BEST)
        IF (KI .EQ. 'BO' .OR. KI .EQ. 'TO') THEN
          IF (KI .EQ. 'BO') OFF = -0.5
          IF (KI .EQ. 'TO') OFF =  0.5
          OMEGA = OMEGA + OFF
          KI = 'SO'
          CALL ANGSET (THETA,OMEGA,CHI,PHI,NATT,ICOL)
          GO TO 120
        ENDIF
        IF (KI .EQ. 'FF') GO TO 200
        OMEGA = OMEGA + STEP*BEST
      ENDIF
C-----------------------------------------------------------------------
C  Chi (Kappa) circle
C-----------------------------------------------------------------------
      IF (KI .EQ. 'SC') THEN
        ICIRCLE = 3
        NPTS = 50
        STEP = 5.0/NPTS
        SSPEED = 20
        CALL GENSCN (ICIRCLE,SSPEED,STEP,NPTS,ISLIT,ICOL)
        CALL PFIT (NPTS,BEST)
        IF (KI .EQ. 'FF') RETURN
        OMEGA = OMEGA + THETA/2.0
        CALL EULKAP (0,OMEGA,CHI,PHI,OMK,RKA,PHIK,ICOL)
        RKA = RKA + STEP*BEST
        CALL EULKAP (1,OMEGA,CHI,PHI,OMK,RKA,PHIK,ICOL)
        OMEGA = OMEGA - THETA/2.0
      ENDIF
C-----------------------------------------------------------------------
C  Phi  circle
C-----------------------------------------------------------------------
      IF (KI .EQ. 'SP') THEN
        ICIRCLE = 4
        NPTS = 50
        STEP = 4.0/NPTS
        SSPEED = 10
  130   CALL GENSCN (ICIRCLE,SSPEED,STEP,NPTS,ISLIT,ICOL)
        CALL PFIT (NPTS,BEST)
        IF (KI .EQ. 'BO' .OR. KI .EQ. 'TO') THEN
          IF (KI .EQ. 'BO') OFF = -2.0
          IF (KI .EQ. 'TO') OFF =  2.0
          PHI = PHI + OFF
          KI = 'SP'
          CALL ANGSET (THETA,OMEGA,CHI,PHI,NATT,ICOL)
          GO TO 130
        ENDIF
        IF (KI .EQ. 'FF') GO TO 200
        PHI = PHI + STEP*BEST
      ENDIF
C-----------------------------------------------------------------------
C  Omega/2theta circles
C-----------------------------------------------------------------------
      IF (KI .EQ. 'WT') THEN
        ICIRCLE = 5
        NPTS = 50
        STEP = 4.0/NPTS
        SSPEED = 20
  140   CALL GENSCN (ICIRCLE,SSPEED,STEP,NPTS,ISLIT,ICOL)
        CALL PFIT (NPTS,BEST)
        IF (KI .EQ. 'BO' .OR. KI .EQ. 'TO') THEN
          IF (KI .EQ. 'BO') OFF = -2.0
          IF (KI .EQ. 'TO') OFF =  2.0
          THETA = THETA + OFF
          OMEGA = OMEGA + 0.5*OFF
          KI = 'WT'
          CALL ANGSET (THETA,OMEGA,CHI,PHI,NATT,ICOL)
          GO TO 140
        ENDIF
        IF (KI .EQ. 'FF') RETURN
        THETA = THETA + STEP*BEST
      ENDIF
  200 CONTINUE
C      write (LPT,99998) ki,theta,omega,chi,phi
C99998 format (' After  ',a,2x,4f8.3)
      RETURN
      END
C
C-----------------------------------------------------------------------
C  Subroutine to find the centroid of the ACOUNT distribution
C-----------------------------------------------------------------------
      SUBROUTINE PFIT (NPTS,BEST)
      INCLUDE 'COMDIF'
      DIMENSION TCOUNT(NSIZE)
      EQUIVALENCE (ACOUNT(9*NSIZE+1), TCOUNT(1))
C-----------------------------------------------------------------------
C  Find the maximum point
C-----------------------------------------------------------------------
      MAX = 0
      SUM = 0.0
      DO 100 I = 1,NPTS
        IF (TCOUNT(I) .GT. MAX) THEN
          MAX = TCOUNT(I)
          IMAX = I
        ENDIF
        SUM = SUM + TCOUNT(I)
  100 CONTINUE     
      IF (MAX .LE. 10) THEN
        KI = 'FF'
        GO TO 200
      ENDIF
C-----------------------------------------------------------------------
C  Find the half-height points on either side
C-----------------------------------------------------------------------
      DO 110 I = IMAX,1,-1
        IF (TCOUNT(I) .LT. MAX/2) THEN
          NBOT = I
          GO TO 120
        ENDIF
  110 CONTINUE
      KI = 'BO'
      BEST = IMAX - NPTS/2
      GO TO 200
  120 DO 130 I = IMAX,NPTS
        IF (TCOUNT(I) .LT. MAX/2) THEN
          NTOP = I
          GO TO 140
        ENDIF
  130 CONTINUE
      KI = 'TO'
      BEST = IMAX - NPTS/2
      GO TO 200
C-----------------------------------------------------------------------
C  Find the point of half sum between NBOT and NTOP
C-----------------------------------------------------------------------
 140  SUM = 0.0
      DO 150 I = NBOT,NTOP
        SUM = SUM + TCOUNT(I)
  150 CONTINUE
      HALF = 0.0
      DO 160 I = NBOT,NTOP
        HALF = HALF + TCOUNT(I)
        IF (HALF .GT. SUM/2.0) GO TO 170
  160 CONTINUE
  170 FRACX = (SUM/2.0 - HALF + TCOUNT(I))/TCOUNT(I)
      BEST = I - 1 + FRACX - NPTS/2 - 0.5
  200 CONTINUE
C      IF (KI .EQ. 'FF' .OR. KI .EQ. 'TO' .OR. KI .EQ. 'BO') THEN
C        write (LPT,99999) KI,imax,max,nbot,ntop,(tcount(i),i=1,50)
C      ENDIF
C99999 format (' imax,max,nbot,ntop',A,4i6/(10f7.0))
      RETURN
      END