C @(#)pair.for	19.1 (ES0-DMD) 02/25/03 13:31:23
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 Massachusetss Ave, Cambridge, 
C MA 02139, USA.
C
C Corresponding 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
       PROGRAM PAIR
C+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
C.IDENT  program PAIR                       A. Lauberts  ESO, Garching  890712
C
C.KEYWORDS  table files, coordinates, alignment, matching
C
C.PURPOSE
C  Match (pair) two point sets with coordinates in table files INA and INB,
C  common set of points form new table file OUTA with added columns,
C  excluded points are appended to the common set,
C  copy of coordinates also in missing places of both sets.
C  Alt 1: coordinates in their own systems
C      2: 2nd system coords transformed to 1st system
C
C.ALGORITHM  common translation vector estimated as
C            mode of histogram of X,Y separations.
C            2nd iteration does small coordinate rotation.
C
C.USAGE  MIDAS application program
C        execute as @@ PAIR P1 P2 P3 P4 P5 P6
C
C.PARAMETERS  input/output
C
C P1  INA  C*8         name of first table file
C P2  INB  C*8         name of second table 
C P3  OUTA C*8         name of output table
C P4  CXY  I*4 array   column nos of coordinates:
C                     1st table (X,Y), 2nd table (X,Y)
C P5  SEP  R*4 array   estimated separations in X, Y between 1st and 2nd tables,
C                           max residual distance after coordinate translation, 
C                           max residual distance after coordinate rotation
C P6  SYS  I*4              0: coordinates in their own systems
C                     1: 2nd system transformed to 1st system
C
C.COMMENTS  Second set of labels have characters 14-16 replaced with
C           column numbers in order to avoid repeated label names;
C           embedded blanks are replaced with underscore (_).
C
C           Max no of objects in each table is limited to 50000.
C
C---------------------------------------------------------------------
       IMPLICIT   NONE
C
       INTEGER      IB(50000), HX(200),HY(200),COL(256),COLA(256)
       INTEGER      JA(50000),JB(50000),CXY(4),STAT,SYS,KUN,KNUL
       INTEGER      MADRID,TID1,TID2,TID3,ACOL,AROW
       REAL         XA(50000),YA(50000),XB(50000),YB(50000)
       REAL         ROW(256),ROWA(256)
       REAL         XAB(50000),YAB(50000),A(4),B(4),SEP(4)
       CHARACTER*60 INA,INB,OUTA
       CHARACTER*8  TYPE
       CHARACTER*80 TEXT*80
       CHARACTER*16 LABEL, FMT, UNIT
       LOGICAL      NULL(256)
C
       INTEGER      IACT
       INTEGER      I, J, L, N, II, JJ
       INTEGER      NCA, NSC, NCB, NC, NS
       INTEGER      NSA, NSB
       INTEGER      NA1, NA2
       INTEGER      NB1, NB2
       INTEGER      NC1
       INTEGER      MA, MB
       REAL         DET
       REAL         S, SS, S1, S2
       REAL         X, Y
       REAL         XM, YM
       REAL         XX, YY
       REAL         XAX, YAY
       INTEGER      IX, IY
       INTEGER      NX, NY
       INTEGER      NX1, NY1
       INTEGER      NX2, NY2
       INTEGER      IEL 	
       INTEGER      LEN
C        
       INCLUDE      'MID_INCLUDE:ST_DEF.INC'
       COMMON       /VMR/MADRID(1)
       INCLUDE      'MID_INCLUDE:ST_DAT.INC'
       DATA         UNIT/' '/
C
       CALL STSPRO('PAIR')
C  get keywords
       CALL STKRDC('IN_A',1,1,60,IACT,INA,KUN,KNUL,STAT)
       CALL STKRDC('IN_B',1,1,60,IACT,INB,KUN,KNUL,STAT)
       CALL STKRDC('OUT_A',1,1,60,IACT,OUTA,KUN,KNUL,STAT)
       CALL STKRDI('INPUTI',1,4,IACT,CXY,KUN,KNUL,STAT)
       CALL STKRDR('INPUTR',1,4,IACT,SEP,KUN,KNUL,STAT)
       CALL STKRDI('INPUTI',5,1,IACT,SYS,KUN,KNUL,STAT)
C
C  read tables
C
       CALL TBTOPN(INA,F_IO_MODE,TID1,STAT)
       CALL TBTOPN(INB,F_IO_MODE,TID2,STAT)
       CALL TBIGET(TID1,NCA,MA,NSC,ACOL,AROW,STAT)
       CALL TBIGET(TID2,NCB,MB,NSC,ACOL,AROW,STAT)
       NC=NCA+NCB
       IF(NC.GT.256) THEN
         CALL STTPUT(' SUM OF COLUMNS > 256',STAT)
         GOTO 999
       ENDIF
       IF(MA.GT.50000.OR.MB.GT.50000) THEN
         CALL STTPUT(' NO OF ROWS > 50000',STAT)
         GOTO 999
       ENDIF
       J=0
       DO I=1,MA
         CALL TBRRDR(TID1,I,2,CXY,ROW,NULL,STAT)
         IF(.NOT.NULL(1).AND..NOT.NULL(2)) THEN
           J=J+1
           JA(J)=I
           XA(J)=ROW(1)
           YA(J)=ROW(2)
         ENDIF
       ENDDO
       MA=J
       J=0
       DO I=1,MB
         CALL TBRRDR(TID2,I,2,CXY(3),ROW,NULL,STAT)
         IF(.NOT.NULL(1).AND..NOT.NULL(2)) THEN
           J=J+1
           JB(J)=I
           XB(J)=ROW(1)
           YB(J)=ROW(2)
         ENDIF
       ENDDO
       MB=J
C+
C
C  estimate projection vector B -> A
C  first form histograms of X and Y differences
C
C-
       S=4./SEP(3)
       DO I=1,200
         HX(I)=0
         HY(I)=0
       ENDDO
       DO J=1,MB
         X=(XB(J)-SEP(1))*S-100.
         Y=(YB(J)-SEP(2))*S-100.
         DO I=1,MA
           IX=XA(I)*S-X
           IF(IX.GT.0.AND.IX.LT.201) THEN
              IY=YA(I)*S-Y
              IF(IY.GT.0.AND.IY.LT.201) THEN
                HX(IX)=HX(IX)+1
                HY(IY)=HY(IY)+1
              ENDIF
           ENDIF
         ENDDO
       ENDDO
C
C  then find highest peak
C
       IX=0
       IY=0
       NX=0
       NY=0
       DO I=2,199
         IF(NX.LT.HX(I)) THEN
           NX=HX(I)
           IX=I
         ENDIF
         IF(NY.LT.HY(I)) THEN
           NY=HY(I)
           IY=I
         ENDIF
       ENDDO
       IF(IX.EQ.2.OR.IX.EQ.199.OR.IY.EQ.2.OR.IY.EQ.199) THEN
         CALL STTPUT(
     1 ' PROJECTION VECTOR TOO LARGE - assume another translation',
     2 STAT)
         GOTO 50
       ENDIF
       S=1./S
       X=(IX-99.5)*S-SEP(1)
       Y=(IY-99.5)*S-SEP(2)
       NX1=HX(IX-1)
       NX2=HX(IX+1)
       NY1=HY(IY-1)
       NY2=HY(IY+1)
       NX=NX1+NX+NX2
       NY=NY1+NY+NY2
       IF(NX.EQ.0.OR.NY.EQ.0) THEN
         CALL STTPUT(' HISTOGRAM ILL DEFINED',STAT)
         N=0
         GOTO 50
       ENDIF
       X=X+S*FLOAT(NX2-NX1)/FLOAT(NX)
       Y=Y+S*FLOAT(NY2-NY1)/FLOAT(NY)
C20    WRITE(6,*) ' finds common points - please wait!'
C+
C
C  find common points.
C  save residual vectors in XAB, YAB arrays
C  first find min residual distance
C
C-
       DO J=1,MB
         IB(J)=0
       ENDDO
       SS=SEP(3)**2
       N=0
       DO I=1,MA
         S1=1.E9
         XAX=XA(I)-X
         YAY=YA(I)-Y
         DO J=1,MB
           S2=(XAX-XB(J))**2 + (YAY-YB(J))**2
           IF(S1.GT.S2) THEN
              S1=S2
              JJ=J
           ENDIF
         ENDDO
         IF(S1.GT.SS.OR.IB(JJ).NE.0) GOTO 40
           IB(JJ)=I
           N=N+1
40         CONTINUE
         XAB(I)=XAX-XB(JJ)
         YAB(I)=YAY-YB(JJ)
       ENDDO

50     CALL STTPUT(' Input table 1: '//INA,STAT)
       CALL STTPUT(' Input table 2: '//INB,STAT)
       CALL STTPUT(' Results: ',STAT)
       WRITE(TEXT,200) X,Y,MA,MB,N
200    FORMAT(' Table 1 - Table 2: (', 2F8.3, ')  ', I5,' +',I5,
     2           ' objects =>',I5,' pairs')
       CALL STTPUT(TEXT,STAT)
       IF(N.LT.2) THEN
         CALL STTPUT(' < 2 COMMON OBJECTS!',STAT)
         GOTO 999
       ENDIF
       IF(N.LT.3) THEN
         CALL STTPUT(' < 3 COMMON OBJECTS - NO ROTATION!',STAT)
         GOTO 60
       ENDIF
C
C  solve for rotation and rescaling
C
       CALL LSQ(XA,YA,XAB,IB,MB,A)
       CALL LSQ(XA,YA,YAB,IB,MB,B)
       WRITE(TEXT,204) (A(I),I=1,3)
204    FORMAT(' residual : DX=',E13.5,' +',E11.3,' * X +',E11.3,' * Y')
       CALL STTPUT(TEXT,STAT)
       WRITE(TEXT,205) (B(I),I=1,3)
205    FORMAT(' rotation : DY=',E13.5,' +',E11.3,' * X +',E11.3,' * Y')
       CALL STTPUT(TEXT,STAT)
       CONTINUE
C+
C
C  Find common points 2nd time;
C  first find min residual distance 
C  with X,Y = individual translation after rotation
C
C-
       DO J=1,MB
         IB(J)=0
       ENDDO
       SS=SEP(4)**2
       N=0
       XM=0.
       YM=0.
       XX=X+A(1)
       YY=Y+B(1)
       DO I=1,MA
         S1=1.E9
         X = XX + A(2)*XA(I) + A(3)*YA(I)
         Y = YY + B(2)*XA(I) + B(3)*YA(I)
         XAX = XA(I)-X
         YAY = YA(I)-Y
         XM = XM+X
         YM = YM+Y
         DO J=1,MB
           S2=(XAX-XB(J))**2 + (YAY-YB(J))**2
           IF(S1.GT.S2) THEN
              S1=S2
              JJ=J
           ENDIF
         ENDDO
         IF(S1.GT.SS.OR.IB(JJ).NE.0) GOTO 55
           IB(JJ)=I
           N=N+1
55         CONTINUE
       ENDDO
       X=XM/MA
       Y=YM/MA
       WRITE(TEXT,200) X,Y,MA,MB,N
       CALL STTPUT(TEXT,STAT)
       IF(N.LT.2) THEN
         CALL STTPUT(' < 2 COMMON OBJECTS!',STAT)
         GOTO 999
       ENDIF
C+
C
C  write merged table with common set of columns
C  second set of labels with column nos in characters 14-16;
C  embedded blanks are replaced by underscore (_).
C
C-
60       N=MA+MB
       CALL STTPUT('*** INFO: writing output table - please wait', 
     2             STAT)
       CALL TBTINI(OUTA,0,F_O_MODE,NC,N,TID3,STAT)
       DO I=1,NCA
         CALL TBLGET(TID1,I,LABEL,STAT)
         CALL TBFGET(TID1,I,FMT,LEN,TYPE,STAT)
         J=INDEX(LABEL,' ')
         IF (J.GT.0 .AND. J.LT.16) THEN
            LABEL(J:J+1)='_A'
         ENDIF 
         CALL TBCINI(TID3,TYPE,1,FMT,UNIT,LABEL,IEL,STAT)
       ENDDO
C
       DO I=1,NCB
         CALL TBLGET(TID2,I,LABEL,STAT)
         CALL TBFGET(TID2,I,FMT,LEN,TYPE,STAT)
         J=INDEX(LABEL,' ')
         IF (J.GT.0 .AND. J.LT.16) THEN
            LABEL(J:J+1)='_B'
         ENDIF 
         CALL TBCINI(TID3,TYPE,1,FMT,UNIT,LABEL,IEL,STAT)
       ENDDO
       DO I=1,NC
         COL(I)=I
       ENDDO
       DET=(1.-A(2))*(1.-B(3))-A(3)*B(2)
       NX=NCA+CXY(3)
       NY=NCA+CXY(4)
       NA1=NCA+1
       NA2=NCA+2
       NS=0
       DO J=1,MB
         I=IB(J)
         IF(I.NE.0) THEN
         II=JA(I)
         JA(I)=-II
         JJ=JB(J)
           CALL TBRRDR(TID1,II,NCA,COL,ROW,NULL,STAT)
           CALL TBRRDR(TID2,JJ,NCB,COL,ROW(NA1),NULL(NA1),STAT)
           IF(SYS.EQ.1) THEN
             X=ROW(NX)+XX
             Y=ROW(NY)+YY
             ROW(NX)=(X*(1.-B(3))+Y*A(3))/DET
             ROW(NY)=(Y*(1.-A(2))+X*B(2))/DET
           ENDIF
           NS=NS+1
           NC1=0
           DO L=1,NC
             IF(.NOT.NULL(L)) THEN
              NC1=NC1+1
              COLA(NC1)=L
              ROWA(NC1)=ROW(L)
             ENDIF
           ENDDO
           CALL TBRWRR(TID3,NS,NC1,COLA,ROWA,STAT)
         ENDIF
       ENDDO
       NSA=NS
       DO I=1,MA
         II=JA(I)
         IF(II.GT.0) THEN
           CALL TBRRDR(TID1,II,NCA,COL,ROW,NULL,STAT)
           X=ROW(CXY(1))
           Y=ROW(CXY(2))
           IF(SYS.EQ.1) THEN
             ROW(NA1)=X*(1.-A(2))-Y*A(3)-XX
             ROW(NA2)=Y*(1.-B(3))-X*B(2)-YY
           ELSE
             ROW(NA1)=X
             ROW(NA2)=Y
           ENDIF
           NSA=NSA+1
           NC1=0
           DO L=1,NCA
             IF(.NOT.NULL(L)) THEN
              NC1=NC1+1
              COLA(NC1)=L
              ROWA(NC1)=ROW(L)
             ENDIF
           ENDDO
           NC1=NC1+1
           COLA(NC1)=NX
           ROWA(NC1)=ROW(NA1)
           NC1=NC1+1
           COLA(NC1)=NY
           ROWA(NC1)=ROW(NA2)
           CALL TBRWRR(TID3,NSA,NC1,COLA,ROWA,STAT)
         ENDIF
       ENDDO
       NX=CXY(3)
       NY=CXY(4)
       NB1=NCB+1
       NB2=NCB+2
       NSB=NSA
       DO J=1,MB
         I=IB(J)
         IF(I.EQ.0) THEN
           JJ=JB(J)
           CALL TBRRDR(TID2,JJ,NCB,COL,ROW,NULL,STAT)
           X=ROW(NX)+XX
           Y=ROW(NY)+YY
           ROW(NB1)=(X*(1.-B(3))+Y*A(3))/DET
           ROW(NB2)=(Y*(1.-A(2))+X*B(2))/DET
           IF(SYS.EQ.1) THEN
             ROW(NX)=ROW(NB1)
             ROW(NY)=ROW(NB2)
           ENDIF
           NSB=NSB+1
           NC1=0
           DO L=1,NCB
             IF(.NOT.NULL(L)) THEN
              NC1=NC1+1
              COLA(NC1)=L+NCA
              ROWA(NC1)=ROW(L)
             ENDIF
           ENDDO
           NC1=NC1+1
           COLA(NC1)=CXY(1)
           ROWA(NC1)=ROW(NB1)
           NC1=NC1+1
           COLA(NC1)=CXY(2)
           ROWA(NC1)=ROW(NB2)
           CALL TBRWRR(TID3,NSB,NC1,COLA,ROWA,STAT)
         ENDIF
       ENDDO
       CALL TBTCLO(TID3,STAT)
999    CALL STSEPI
       END
C
       SUBROUTINE LSQ(X,Y,AB,IB,MB,B)
C+++
C  LSQ fit of residuals
C  AB = B1 + B2*X + B3*Y
C  to <= MB points (X,Y)
C----
       IMPLICIT   NONE       
       REAL       X(1)
       REAL       Y(1)
       REAL       AB(1)
       INTEGER    IB(1)
       INTEGER    MB
       REAL       B(1)
C     
       INTEGER    I, J, K, L, N
       INTEGER    N1
C
       DOUBLE PRECISION  A(4,4)
       DOUBLE PRECISION  C(4)
C
       N=3
       N1=N+1
       DO I=1,N1
         DO J=1,N1
           A(I,J)=0.D0
         ENDDO
         B(I)=0.
       ENDDO
       DO K=1,MB
         L=IB(K)
         IF(L.NE.0) THEN
           C(1)=1.
           C(2)=X(L)
           C(3)=Y(L)
           C(4)=AB(L)
           DO I=1,N1
             DO J=1,N1
              A(I,J)=A(I,J)+C(I)*C(J)
             ENDDO
           ENDDO
         ENDIF
       ENDDO
       CALL SIMUL(N,A,B)
       RETURN
       END
