C @(#)fitell1.for	17.1.1.1 (ESO-DMD) 01/25/02 17:19:20
C===========================================================================
C Copyright (C) 1995 European Southern Observatory (ESO)
C
C This program is free software; you can redistribute it and/or 
C modify it under the terms of the GNU General Public License as 
C published by the Free Software Foundation; either version 2 of 
C the License, or (at your option) any later version.
C
C This program is distributed in the hope that it will be useful,
C but WITHOUT ANY WARRANTY; without even the implied warranty of
C MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
C GNU General Public License for more details.
C
C You should have received a copy of the GNU General Public 
C License along with this program; if not, write to the Free 
C Software Foundation, Inc., 675 Massachusetts Ave, Cambridge, 
C MA 02139, USA.
C
C Correspondemce concerning ESO-MIDAS should be addressed as follows:
C	Internet e-mail: midas@eso.org
C	Postal address: European Southern Observatory
C			Data Management Division 
C			Karl-Schwarzschild-Strasse 2
C			D 85748 Garching bei Muenchen 
C			GERMANY
C===========================================================================
C
C ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
C.COPYRIGHT: Copyright (c) 1987 European Southern Observatory,
C                                         all rights reserved
C.IDENTIFICATION: FITELL
C.PURPOSE:   Fit ellipses to a 2-dim image
C.LANGUAGE:  F77+ESOext
C.AUTHOR:    Andris Lauberts
C.KEYWORDS:  Ellipse fitting, display fit
C.ALGORITHM: Search for points selected on radius and intensity,
C            demand at least three points in 3x3 pixel region,
C            least square fit to the rectangular coords of these points,
C            retrieve ellips parameters, display points + fit
C.VERSION:   890717 AL   creation
C            890727 AL   revised
C            900315 MP   SX ---> ST
C 
C 010201		last modif
C 
C ---------------------------------------------------------------------
 
      PROGRAM FITELL1
C
      IMPLICIT  NONE
C
      INTEGER   MADRID,KUN,KNUL
      INTEGER   I,IAV
      INTEGER   M
      INTEGER   NAXIS
      INTEGER   NPIX(2)
      INTEGER   INO,ONO
      INTEGER   STAT
      INTEGER*8   PNTRA,PNTRB
C
      REAL      STEP(2),START(2)
      REAL      C(4)
      REAL      CENTER(2)
      REAL      RLIM
      REAL      XF(10000),YF(10000)
      REAL      ISOP(2),MISO,P,A,B,DEG,ERR
C
      DOUBLE PRECISION DSTART(2),DSTEP(2)
      CHARACTER IDENT*72,CUNIT*48,TEXT*80
      CHARACTER INFR*60,OUTFR*60
C
      INCLUDE 'MID_INCLUDE:ST_DEF.INC'
      COMMON / VMR /MADRID(1)
      INCLUDE 'MID_INCLUDE:ST_DAT.INC'
C
      DEG=180./3.1415926
C
C *** start code
C
      CALL STSPRO('FITELL1 ')                      ! set up Midas environment
C
C ** get keywords
C  
      CALL STKRDC('IN_A',1,1,60,IAV,INFR,KUN,KNUL,STAT) !  get inframe name
      CALL STKRDC('OUT_A',1,1,60,IAV,OUTFR,KUN,KNUL,STAT)!  get outframe name
      CALL STKRDR('INPUTR',4,2,IAV,ISOP,KUN,KNUL,STAT)     !  isophote bounds
      CALL STKRDR('INPUTR',1,2,IAV,CENTER,KUN,KNUL,STAT)   !  center coords
      CALL STKRDR('INPUTR',3,1,IAV,RLIM,KUN,KNUL,STAT)    !  limiting radius 
C
C *** get the images
C
C
      CALL STIGET(INFR,D_R4_FORMAT,F_I_MODE,F_IMA_TYPE,2,NAXIS,
     +     NPIX,DSTART,DSTEP,IDENT,CUNIT,PNTRA,INO,STAT)
C
      CALL STIGET(OUTFR,D_R4_FORMAT,F_U_MODE,F_IMA_TYPE,2,NAXIS,
     +     NPIX,DSTART,DSTEP,IDENT,CUNIT,PNTRB,ONO,STAT)
C
C *** Search for points within radius and isophotal window
C     For found M points retrieve only their rectangular 
C     coordinates in XF, YF arrays
C
      DO 100 I =1,2
         START(I) = SNGL(DSTART(I))
         STEP(I)  = SNGL(DSTEP(I))
100   CONTINUE
         CALL SELECT(MADRID(PNTRA),MADRID(PNTRB),
     +       	    NPIX,START,STEP,CENTER,XF,YF,ISOP,RLIM,M)
C
C *** Check on the actual number of points found
C
         IF (M.GT.10000) THEN
            CALL STTPUT('*** WARNING:  > 10000 Points found '//
     +                   ' results unreliable',STAT)
            CALL STTPUT('*** ADVICE:  make window smaller',STAT)
           GOTO 999
         END IF
         IF (M.LT.10) THEN
            CALL STTPUT('*** WARNING: < 10 points found '//
     +                   ' results unreliable',STAT)
            CALL STTPUT('*** ADVICE:  make window larger',STAT)
           GOTO 999
         END IF
C
C *** do least square fit; ellipse fit coefficients in array C
C
         CALL LSQ(XF,YF,M,C,ERR)
C
C *** solve for position angle P (0 on X-axis, increasing towards Y-axis)
C     and semi-diameters A, B.
C
        CALL SOLVE(C,P,A,B)
C
C *** mark found datapoints and ellipse in outframe
C
         MISO=(ISOP(1)+ISOP(2))/2.
         CALL DISPLAY(MADRID(PNTRB),NPIX,START,STEP,CENTER,P,A,B)
C
C *** Print table
C
      CALL STTPUT(' ',STAT)
      CALL STTPUT('*** Summary of the Results ***',STAT)
      CALL STTPUT(
     +  'Mean isoph   Semi-diameters      Posangle    #points  Error',
     +STAT)
      P=P*DEG
      WRITE (TEXT,9000) MISO,A,B,P,M,ERR
9000  FORMAT (G9.3,3F10.2,I8,F10.3)
      CALL STTPUT(TEXT,STAT)
      XF(1)=CENTER(1)
      XF(2)=CENTER(2)
      XF(3)=A
      XF(4)=B
      XF(5)=P
      CALL STDWRR(ONO,'ELLPAR',XF,1,5,KUN,STAT)
      CALL STFCLO(ONO,STAT)
C
999   CALL STSEPI
      END
C
C++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
C.IDENTIFICATION: SELECT
C.AUTHOR:   A. Lauberts
C.PURPOSE:  Selects image pixels in range of intensities and limiting radius
C.INPUT     A       inframe image intensities                   
C           B       outframe image intensities                  output
C           NPIX    number of pixels/axis
C           START   start X, Y coords
C           STEP    step size in X, Y
C           CENTER  center X, Y
C           XF      X coord of found points relative to center  output
C           YF      Y coord of found points relative to center  output
C           ISOP    lower, upper isophotal levels                keyword
C           RLIM    maximum radius used for search              keyword
C           M       number of points found                      output
C---------------------------------------------------------------------
      SUBROUTINE SELECT(A,B,NPIX,START,STEP,CENTER,XF,YF,ISOP,
     + RLIM,M)
C
      IMPLICIT  NONE
      REAL      A(1)
      REAL      B(1)
      INTEGER   NPIX(2)
      REAL      START(2)
      REAL      STEP(2)
      REAL      CENTER(2)
      REAL      XF(1)
      REAL      YF(1)
      REAL      ISOP(2)
      REAL      RLIM
      INTEGER   M

      INTEGER   I, II
      INTEGER   N,STAT
      INTEGER   NPX1, NPX2
      INTEGER   J
      INTEGER   K
      INTEGER   L
      REAL      R2, RLM2
      REAL      STC1, STC2, STP1, STP2
      REAL      X
      REAL      Y, Y2
      REAL      AN,AM,BM
      REAL      LOLIM,UPLIM
      CHARACTER TEXT*80
C
 9000 FORMAT ('Isop limits=',F8.2,1X,F8.2,' Rad=',F8.2,
     + '  #Pnts=',I4)
C
      LOLIM = ISOP(1)
      UPLIM = ISOP(2)
C
C *** Search intensity array for filtered values,
C     simultaneously check radii limit,
C     if true put corresponding coords in XF and YF arrays.
C     Skip two first and two last scan lines.
C
C     Also mark found pixels in outframe by value 32000.
C
      NPX1 = NPIX(1)                          ! put before `N=NPX1*2' 
      NPX2 = NPIX(2)
      RLM2 = RLIM**2
      STC1 = START(1)-CENTER(1)
      STC2 = START(2)-CENTER(2)
      STP1 = STEP(1)
      STP2 = STEP(2)
      M = 0
      N = NPX1*2
      DO J = 3,NPX2-2
        Y = STC2+(J-1)*STP2
        Y2= Y**2
        DO I = 1,NPX1
           X = STC1+(I-1)*STP1
           R2= X**2+Y2
           N = N+1
           AN= A(N)
           IF (R2.LT.RLM2 .AND. AN.LT.UPLIM .AND. 
     +      AN.GT.LOLIM) THEN
            M      = M + 1
           IF(M.GT.10000) RETURN
            XF(M)  = X
            YF(M)  = Y
            B(N)  = 32000.
           END IF
        ENDDO
      ENDDO
      WRITE (TEXT,9000) ISOP(1),ISOP(2),RLIM,M
      CALL STTPUT(TEXT,STAT)
C
C  Go through the image again. 
C  This time demand that a 3x3 pixel region should contain 
C  at least 3 selected points, including the central point,
C  expected to join a well behaved contour line.
C
      L = 0
      M = 0
      N = NPX1*2
      DO J = 3,NPX2-2
       DO I = 1,NPX1
         N = N+1
         IF(B(N).EQ.32000.) THEN
           L = L+1
           K = 0
           DO II = N-1,N+1
             IF(B(II-NPX1).EQ.32000.) K = K+1
             IF(B(II     ).EQ.32000.) K = K+1
             IF(B(II+NPX1).EQ.32000.) K = K+1
           ENDDO
           IF(K.GT.2) THEN
             M = M+1
             XF(M) = XF(L)
             YF(M) = YF(L)
           ELSE
             AM = A(N)
             BM = B(N)
             B(N) = A(N)
             BM = B(N)
           ENDIF
         ENDIF
       ENDDO
      ENDDO
      WRITE (TEXT,9000) ISOP(1),ISOP(2),RLIM,M
      CALL STTPUT(TEXT,STAT)
      END
C
C
C+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
C.IDENTIFICATION: SOLVE
C.PURPOSE:        get position angle P and semi-diameters A, B 
C       	  for ellipse fit to an image isophote
C
C.AUTHOR:         A. Lauberts
C--------------------------------------------------------------------
       SUBROUTINE SOLVE(C,P,A,B)
C
       IMPLICIT NONE
       REAL	C(4)
       REAL     P
       REAL     A
       REAL     B
C
       REAL     SP2
       REAL     A2
       REAL     B2
       REAL     P2,C1,C2,C3,PI,PI2,PIH
C
       PI=3.1415926
       PIH=PI/2.
       PI2=PI*2.
C
C  Let ellipse equation in quadratic form be
C
C       C1*X*X+C2*X*Y+C3*Y*Y = 1
C
C  Let ellipse equation in normal form be
C
C       r*U*U + s*V*V = 1
C
C  with  1/r = A*A  and  1/s = B*B.
C
C  (U,V) and (X,Y) coordinate systems are transformed by relative rotation
C
C       U = X cos P + Y sin P
C       V =-X sin P + Y cos P
C
C  using this transformation and identifying the two ellipse equation forms
C  gives P, A, B expressed in C's:
C
       C1=C(1)
       C2=C(2)
       C3=C(3)
       IF(C1.NE.C3) THEN
         P2=C2/(C1-C3)
         P2=ATAN(P2)
         SP2=SIN(P2)
       ELSE
         P2=PIH
         SP2=1.
       ENDIF
       IF(SP2.NE.0.) THEN
           A2=C1+C3+C2/SP2
           B2=C1+C3-C2/SP2
           IF(A2.GT.0..AND.B2.GT.0.) THEN
       	A=SQRT(2./A2)
       	B=SQRT(2./B2)
       	IF(A.GT.B) THEN
       	  P=P2/2.
       	ELSE
       	  A2=A
       	  A=B
       	  B=A2
       	  P=P2/2.+PIH
       	ENDIF
       	IF(P.LT.0.) P=P+PI
       	IF(P.GT.PI) P=P-PI
            ELSE
       	WRITE(6,*) 'A2 or B2 < 0, A2,B2=',A2,B2
       	RETURN
           ENDIF
       ELSE
           WRITE(6,*) 'SP2 = 0'
           A2=C1+C3
           IF(A2.GT.0.) THEN
             A=SQRT(2./A2)
             B=A
             P=0.
           ELSE
             WRITE(6,*) 'A2 < 0, A2= ',A2
             RETURN
           ENDIF
       ENDIF
       END
C
C+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
C.IDENTIFICATION: DISPLAY
C.PURPOSE:        Marks rectangular image coordinates and labels in
C                 outframe (original copy of inframe) consisting of:
C                 set of M points in rectangular coords given by XF and YF,
C                 ellips given by least square parameters P = posangle, 
C       	  A = semi-major diameter and B = semi-minor diameter
C.AUTHOR:         A. Lauberts
C.NOTE:       	  The isophotal points have already been marked in SELECT
C -------------------------------------------------------------------
      SUBROUTINE DISPLAY(DISP,NPIX,START,STEP,CENTER,P,A,B)
C
      IMPLICIT NONE
      REAL     DISP(1)
      INTEGER  NPIX(2)
      REAL     START(2)
      REAL     STEP(2)
      REAL     CENTER(2)
      REAL     P,A,B
C
      INTEGER  I, IX, IY, II
      REAL     DISP1(400)
      REAL     TH,XX,YY,X,Y,RAD,COSP,SINP
C
       RAD = 3.1415926/180.
       COSP=COS(P)
       SINP=SIN(P)
C  
C *** mark the ellips in one degree angular steps;
C     data of found points from previous step are lost if they coincide
C
      DO 30 I = 1,361
         TH     = (I-1)*RAD
         X      = A*COS(TH)
         Y      = B*SIN(TH)
         XX     = X*COSP-Y*SINP
         YY     = X*SINP+Y*COSP
         IX     = IFIX((XX+CENTER(1)-START(1))/STEP(1)+1.5)
         IY     = IFIX((YY+CENTER(2)-START(2))/STEP(2)+1.5)
         II     = (IY-1)*NPIX(1) + IX
         DISP1(I) = DISP(II)
         DISP(II) = 0.
   30 CONTINUE
      END
C
       SUBROUTINE LSQ(X,Y,M,C,E1)
C+++++++++
C  LSQ fit of ellipse equation with coeffs C
C  
C     C(1)*X*X + C(2)*X*Y + C(3)*Y*Y = 1
C
C  to max M points (X,Y)
C
C  2nd iteration with restricted set of points within smaller
C  error bounds
C-------------
       IMPLICIT  NONE
       REAL      X(1)
       REAL      Y(1)
       INTEGER   M
       REAL      C(4)
       REAL      E1
C
       INTEGER   N, N1
       INTEGER   J
       INTEGER   K
       INTEGER   L
       INTEGER   I
       REAL      E2
       DOUBLE PRECISION  A(4,4),B(4),S(4)
C
       N=3
       N1=N+1
       DO I=1,N1
         DO J=1,N1
           A(I,J)=0.D0
         ENDDO
         S(I)=0.D0
         C(I)=0.
       ENDDO
       DO K=1,M
           B(1)=X(K)*X(K)
           B(2)=X(K)*Y(K)
           B(3)=Y(K)*Y(K)
           B(4)=1.
           DO I=1,N1
             DO J=1,N1
       	A(I,J)=A(I,J)+B(I)*B(J)
             ENDDO
           ENDDO
           S(1)=S(1)+B(1)
           S(2)=S(2)+B(2)
           S(3)=S(3)+B(3)
       ENDDO
       CALL SIMUL(N,A,C)
C  squared error E2 per point, 1st iteration
       S(4)=M
       E2 = 1.-(C(1)*S(1)+C(2)*S(2)+C(3)*S(3))/S(4)
       IF(E2.GE.0.) THEN
         E1 = SQRT(E2)
       ELSE
         E1=0.
         WRITE(6,*) '*** Negative error'
       ENDIF
       WRITE(6,*) 'Error, 1st iteration:',E1
C  2nd selection with E2 < 1st E2/2.
       E1 = E2/2.
       L=0
       DO K=1,M
         E2=(1.-C(1)*X(K)*X(K)-C(2)*X(K)*Y(K)-C(3)*Y(K)*Y(K))**2
         IF(E2.LT.E1) THEN
           L=L+1
           X(L)=X(K)
           Y(L)=Y(K)
         ENDIF
       ENDDO
C
       M=L
       IF(M.LT.10) RETURN
       DO I=1,N1
         DO J=1,N1
           A(I,J)=0.D0
         ENDDO
         S(I)=0.D0
         C(I)=0.
       ENDDO
       DO K=1,M
           B(1)=X(K)*X(K)
           B(2)=X(K)*Y(K)
           B(3)=Y(K)*Y(K)
           B(4)=1.
           DO I=1,N1
             DO J=1,N1
       	A(I,J)=A(I,J)+B(I)*B(J)
             ENDDO
           ENDDO
           S(1)=S(1)+B(1)
           S(2)=S(2)+B(2)
           S(3)=S(3)+B(3)
       ENDDO
       CALL SIMUL(N,A,C)
C  squared error E2 per point, second iteration
       S(4)=M
       E2 = 1.-(C(1)*S(1)+C(2)*S(2)+C(3)*S(3))/S(4)
       IF(E2.GE.0.) THEN
         E1=SQRT(E2)
       ELSE
         E1=0.
         WRITE(6,*) '*** Negative error'
       ENDIF
       WRITE(6,*) 'Error, 2nd iteration:',E1
       END