C----
C---- $Id: general_fit.f,v 1.2 1995/11/10 14:16:21 hooft Exp $
C----
      SUBROUTINE GENERAL_FIT (X,Y,SX,SY,N,A,B,CHI2,Q)
C----
C---- B=SLOPE
C----
      REAL X(N),Y(N),SX(N),SY(N),A,B
      REAL VAR(2),ERR(2)
      VAR(1)=A
      VAR(2)=B
      ERR(1)=A/100.0
      ERR(2)=B/100.0
      WRITE(*,*) 'SIMPLEX OPTIMIZATION OF FIT...'
      WRITE(*,*) 'INITIAL VALUES FOR A AND B: ',A,B
      CALL SIM(VAR,ERR,2,0.00001,S)
      A=VAR(1)
      B=VAR(2)
      WRITE(*,*) 'FINAL VALUES FOR A AND B: ',A,B
      WRITE(*,*) 'CHI SQUARED: ',S
      CHI2=S
      Q=GAMMQ(0.5*(N-2),0.5*CHI2)
      RETURN
      END

      SUBROUTINE SIM (VAR,ERR,NVAR,CRIT,S)
C----
C---- Simplex optimization
C----
C---- INPUT:
C---- VAR(NVAR)      = Array of parameters to be changed
C---- ERR(NVAR)      = Array of expected deviations in the parameters
C---- CRIT           = Convergence criterium
C---- FUNC           = The function to be fitted
C---- OUTPUT:
C---- VAR(NVAR)      = Array of optimized parameters
C---- S              = Sum of squares of residuals
C----
      IMPLICIT      NONE
      INTEGER       NVAR, NDIM, I, J, NCYCL, IWORST, IDIM, IVAR
      REAL          VAR(NVAR), ERR(NVAR), CRIT, S, S_FUNC, BESTS,
     +              WORSTS, ENLARGES, FINAL
      REAL          SIMPLEX(21,21), NEWS, CMASS(21), VEC(21)
      EXTERNAL      S_FUNC

      NDIM=NVAR+1
      IF (NDIM.GT.21) THEN
         STOP 'Simplex with too many variables.'
      END IF
C----
C---- SET UP THE SIMPLEX
C----
      DO I=1,NDIM
         DO J=1,NVAR
            SIMPLEX(I,J)=VAR(J)
            IF (I.EQ.J+1) SIMPLEX(I,J)=SIMPLEX(I,J)+ERR(J)
         END DO
      END DO

      DO I=1,NDIM      
        DO J=1,NVAR
          VAR(J)=SIMPLEX(I,J)
        ENDDO
        S=S_FUNC(VAR)
        SIMPLEX(I,NDIM)=S
      ENDDO
C===================================================================
      NCYCL=0
100   CONTINUE
C----
C---- FIND WORST AND BEST
C----
      BESTS=1.E10
      WORSTS=0
      IWORST=0
      DO IDIM=1,NDIM
        IF (SIMPLEX(IDIM,NDIM).GT.WORSTS) THEN
          IWORST=IDIM
          WORSTS=SIMPLEX(IDIM,NDIM)
        ENDIF
        BESTS=MIN(BESTS,SIMPLEX(IDIM,NDIM))
      ENDDO
      IF ((WORSTS-BESTS)/BESTS.LT.CRIT) GOTO 9999
C----
C---- Find center of mass for rest of simplex.
C----
      DO IVAR=1,NVAR
         CMASS(IVAR)=0
      ENDDO
      DO IDIM=1,NDIM      
         IF (IWORST.NE.IDIM) THEN
            DO IVAR=1,NVAR
               CMASS(IVAR)=CMASS(IVAR)+SIMPLEX(IDIM,IVAR)
            END DO
         END IF
      END DO
      DO IVAR=1,NVAR
         CMASS(IVAR)=CMASS(IVAR)/(NDIM-1)
      END DO
C----
C---- Calculate vector from worst to center.
C----
      DO IVAR=1,NVAR
         VEC(IVAR)=CMASS(IVAR)-SIMPLEX(IWORST,IVAR)
      END DO
C----
C---- INVERT WORST POINT OF SIMPLEX THROUGH CENTER OF MASS
C----
      DO IVAR=1,NVAR
         SIMPLEX(IWORST,IVAR)=CMASS(IVAR)+VEC(IVAR)
      END DO

      DO J=1,NVAR
         VAR(J)=SIMPLEX(IWORST,J)
      END DO
      NEWS=S_FUNC(VAR)
C----
C---- Now we have to make some decisions
C----
      IF (NEWS.GE.WORSTS) THEN
C-------
C------- Shrink the simplex
C-------
         DO IVAR=1,NVAR
            SIMPLEX(IWORST,IVAR)=CMASS(IVAR)-0.5*VEC(IVAR)
         END DO
         DO J=1,NVAR
            VAR(J)=SIMPLEX(IWORST,J)
         END DO
         NEWS=S_FUNC(VAR)
      ELSE IF (NEWS.LT.BESTS) THEN
C-------
C------- Enlarge the simplex
C-------
         DO IVAR=1,NVAR
            SIMPLEX(IWORST,IVAR)=CMASS(IVAR)+2*VEC(IVAR)
         END DO
         DO J=1,NVAR
            VAR(J)=SIMPLEX(IWORST,J)
         END DO
         ENLARGES=S_FUNC(VAR)
         IF (ENLARGES.GT.NEWS) THEN
C----------
C---------- Put the not-enlarged one back.
C----------
            DO IVAR=1,NVAR
               SIMPLEX(IWORST,IVAR)=CMASS(IVAR)+VEC(IVAR)
            END DO
         ELSE
            NEWS=ENLARGES
         END IF
      END IF
      SIMPLEX(IWORST,NDIM)=NEWS
c     5     FORMAT(X,A,F7.3,A,I3,A,2F9.6)
      NCYCL=NCYCL+1
      GOTO 100
9999  CONTINUE
      WRITE(*,*) 'Convergence was reached after',NCYCL,' cycles'
C----
C---- Find center of mass
C----
      DO IVAR=1,NVAR
         CMASS(IVAR)=0
      END DO
      DO IDIM=1,NDIM      
         DO IVAR=1,NVAR
            CMASS(IVAR)=CMASS(IVAR)+SIMPLEX(IDIM,IVAR)
         END DO
      END DO
      DO IVAR=1,NVAR
         SIMPLEX(1,IVAR)=CMASS(IVAR)/NDIM
      END DO
      DO J=1,NVAR
         VAR(J)=SIMPLEX(1,J)
      END DO
      FINAL=S_FUNC(VAR)
      S=FINAL
      RETURN
      END

      REAL FUNCTION S_FUNC(VAR)
      INCLUDE  'SCATTER.INC'
      REAL     AA, BB, PDX, PDY, VAR(2)
      INTEGER  J
      REAL     S, NI, TI

      S=0
      AA=VAR(2)
      BB=VAR(1)
      DO J=1,NDATA
         PDX=DATASDX(J)
         PDY=DATASDY(J)
         TI = (DATAY(J) - AA*DATAX(J) - BB) **2
         NI = (AA*PDX)**2 - 2*AA*RHO*PDX*PDY + PDY**2
         S=S+ TI/NI
      ENDDO
      S_FUNC=S
      RETURN
      END
C      REAL VAR(2),N1,N2,S,PDX,PDY,XN,DIS,S_FUNC
C      INTEGER J
C      S=0
C      DO J=1,NDATA
C        PDX=DATASDX(J)
C        PDY=DATASDY(J)
C        N1=-VAR(2)*PDX/PDY
C        N2=1.0
C        XN=SQRT(N1**2+N2**2)
C        N1=N1/XN
C        N2=N2/XN
C        DIS=N1*DATAX(J)/PDX+N2*(DATAY(J)-VAR(1))/PDY
C        S=S+DIS**2
C      ENDDO
C      S_FUNC=S
C      RETURN
C      END

