v3.0.3 - 11 May 2016 

C$ Procedure
 
      SUBROUTINE FIND_SLOPES(PMX,NTMP,NPIX,QSZ,NSZ,DN1,SP,CP,PUSE,
     .                    APR,LAMBDA,PHI,SK,TUSE,TMPL,RESIDUAL,SS)

C$ Abstract
C
C     This subroutine sets up the estmation that determines the slope and 
C     relative albedo for each pixel in a maplet by minimzing the brightness
C     residuals at each point.  The actual heavy lifting for each pixel is 
C     done by the subroutine FIND_TMPL.  This subroutine determines the 
C     residuals and the weights for the estimation.  Note that lower resolution 
C     images are weighted less as are those with lower incidence angles and 
C     very dark pixels.  This weighting is not reflected in the "RMS brightness
C     residuals" that are displayed when LITHOS calls this subroutine, so that
C     value may increase slightly with iteration.
C
C     There is no reason why these two subroutines could not be combined, but
C     for the moment we are taking an "If it ain't broke ..." attitude.    
C
C$ Disclaimer
C
C
C$ Required_Reading
C
C     R.W. Gaskell, et.al, "Characterizing and navigating small bodies
C           with imaging data", Meteoritics & Planetary Science 43,
C           Nr 6, 1049-1061 (2008)
C
C$ Declarations

      IMPLICIT NONE
      
      INTEGER               PMX
      INTEGER               NTMP
      
      DOUBLE PRECISION      SP(3,PMX)
      DOUBLE PRECISION      CP(3,PMX)
      DOUBLE PRECISION      APR(3)      
      DOUBLE PRECISION      ALPHA
      DOUBLE PRECISION      RPD
      DOUBLE PRECISION      VDOT
      DOUBLE PRECISION      ILLUM
      DOUBLE PRECISION      GAMMA
      DOUBLE PRECISION      ETA
      DOUBLE PRECISION      MU
      DOUBLE PRECISION      NU
      DOUBLE PRECISION      LAMBDA(PMX)
      DOUBLE PRECISION      PHI(PMX)
      DOUBLE PRECISION      SK(PMX)
      DOUBLE PRECISION      WGT(PMX)
      DOUBLE PRECISION      RSD(PMX)
      DOUBLE PRECISION      RESIDUAL
      DOUBLE PRECISION      Z1, Z2, Z3, Z4, Z5, Z6
      DOUBLE PRECISION      W1, W2
      DOUBLE PRECISION      SS
      
      REAL*4                DN1(5000,5000)
      REAL*4                TMPL(-NTMP:NTMP,-NTMP:NTMP,3)

      INTEGER               NPIX
      INTEGER               QSZ
      INTEGER               NSZ
      INTEGER               I
      INTEGER               J
      INTEGER               K
      INTEGER               N
      INTEGER               K1
      INTEGER               N1
      INTEGER               N2
      INTEGER               I0
      INTEGER               J0

      LOGICAL               TUSE(-NTMP:NTMP,-NTMP:NTMP)
      LOGICAL               USE(PMX)
      LOGICAL               PUSE(PMX)
 
C$ Variable_I/O
C
C     Variable  I/O  Description
C     --------  ---  --------------------------------------------------
C     PMX
C     NTMP
C     NPIX
C     QSZ
C     NSZ
C     DN1
C     SP
C     CP
C     PUSE
C     APR
C     LAMBDA
C     PHI
C     SK
C     TUSE
C     TMPL
C     RESIDUAL
C     SS
C
C$ File_I/O
C
C     Filename                      I/O  Description
C     ----------------------------  ---  -------------------------------
C     None
C
C$ Restrictions
C
C
C$ Software_Documentation
C
C     OSIRIS-REx Stereophotoclinometry Software Design Document
C     OSIRIS-REx Stereophotoclinometry Software User's Guide
C
C$ Author_and_Institution
C
C     R.W. Gaskell    (PSI)
C
C$ Version
C
C
C
C$ SPC_functions_called
C     ILLUM
C
C$ SPC_subroutines_called
C     FIND_TMPL
C
C$ SPICELIB_functions_called
C     RPD
C     VDOT
C
C$ SPICELIB_subroutines_called
C     None
C 
C$ Called_by_SPC_Programs
C     LITHOS
C 

      MU=0.75
      NU=0.05

      Z1=0          % counter for calculating residual
      Z3=0
      W1=0
      N1=0
      DO I=-QSZ,QSZ
      DO J=-QSZ,QSZ
        N=0
        GAMMA=SQRT(1+TMPL(I,J,1)**2+TMPL(I,J,2)**2)
        ETA=1+TMPL(I,J,3)

% for every image
        DO K=1,NPIX
          N=N+1
          USE(K)=.FALSE.
          IF(PUSE(K)) THEN
            K1=(N-1)/NSZ
            N2=N-1-NSZ*K1

            I0=(QSZ+1)*(1+2*N2)
            J0=(QSZ+1)*(1+4*K1)
            Z5=(SP(3,K)+TMPL(I,J,1)*SP(1,K)+TMPL(I,J,2)*SP(2,K))/GAMMA
            Z6=(CP(3,K)+TMPL(I,J,1)*CP(1,K)+TMPL(I,J,2)*CP(2,K))/GAMMA
            WGT(K)=0

            IF((Z5.GT.0).AND.(Z6.GT.0)) THEN
              IF(DN1(I+I0,J+J0).GT.0) THEN

% Weighting for low incidence angle
                WGT(K)=SK(K)*(1-MU*SP(3,K)**2)/LAMBDA(K)**2
                Z2=DN1(I+I0,J+J0)

% Weighting for brightness
                WGT(K)=WGT(K)*Z2**4/(Z2**4+NU**4)

% Phase
                ALPHA=ACOS(VDOT(CP(1,K),SP(1,K)))/RPD()

                RSD(K)=DN1(I+I0,J+J0)
     .                -LAMBDA(K)*ETA*ILLUM(Z5,Z6,ALPHA)-PHI(K)
                USE(K)=.TRUE.
              ENDIF
            ENDIF
          ENDIF
        ENDDO

        CALL FIND_TMPL(PMX,NPIX,RSD,SP,CP,APR,LAMBDA,WGT,
     .                 TMPL(I,J,1),TMPL(I,J,2),TMPL(I,J,3),
     .                 USE,PUSE,TUSE(I,J),Z2,W2,Z4)
          
        Z1=Z1+Z2
        W1=W1+W2
        IF(Z4.NE.0) THEN
          Z3=Z3+Z4
          N1=N1+1
        ENDIF

      ENDDO
      ENDDO

      RESIDUAL =  SQRT(Z1/W1)
      SS=SQRT(Z3/N1)

      Z1= 1.D10
      Z2=-1.D10
      Z3=0
      N1=0
      DO I=-QSZ,QSZ
      DO J=-QSZ,QSZ
      IF(TUSE(I,J)) THEN
        Z1=MIN(Z1,TMPL(I,J,3))
        Z2=MAX(Z2,TMPL(I,J,3))
        Z3=Z3+TMPL(I,J,3)
        N1=N1+1
      ENDIF
      ENDDO
      ENDDO
      Z3=Z3/N1
      Z1=1.01*MAX(1.D0,(Z2-Z3),(Z3-Z1))
      DO I=-QSZ,QSZ
      DO J=-QSZ,QSZ
      IF(TUSE(I,J)) THEN
        TMPL(I,J,3)=(TMPL(I,J,3)-SNGL(Z3))/SNGL(Z1)
      ENDIF
      ENDDO
      ENDDO

      RETURN
      END