      PROGRAM MAIN
      IMPLICIT NONE
      INTEGER                                           :: NT,NA,NR
      DOUBLE PRECISION,DIMENSION (:,:),ALLOCATABLE      :: GP,G0,R0,P0,A0,GA0,LHS
      DOUBLE PRECISION,DIMENSION (:,:),ALLOCATABLE      :: PP,PPI
      DOUBLE PRECISION,DIMENSION (:,:),ALLOCATABLE      :: P0I,GPG0,GPG0I
      DOUBLE PRECISION,DIMENSION (:),ALLOCATABLE        :: K0,WVP,RHS
      INTEGER,DIMENSION (:),ALLOCATABLE                 :: ID1,ID2,CAND,GEND
      INTEGER,DIMENSION (:,:),ALLOCATABLE               :: NONR
      DOUBLE PRECISION, DIMENSION (:),ALLOCATABLE       :: V0,VV1,VV2,VV0
      DOUBLE PRECISION                                  :: RATIO
      INTEGER                                           :: NS1,NS2
      INTEGER                                           :: I,J,K,II,JJ,IWK
      CHARACTER                                         :: DATE*8,TIME*12
      INTERFACE
        SUBROUTINE AMRX (ID1,ID2,A,NA)
          IMPLICIT NONE
          INTEGER,INTENT(IN),DIMENSION (:)              :: ID1,ID2
          DOUBLE PRECISION,INTENT(OUT),DIMENSION (:,:)  :: A
          INTEGER,INTENT(IN)                            :: NA
        END SUBROUTINE AMRX
        SUBROUTINE KRON (A,B,C,N1,N2,N3,N4)
          IMPLICIT NONE
          DOUBLE PRECISION,INTENT(IN),DIMENSION (:,:)   :: A,B
          DOUBLE PRECISION,INTENT(OUT),DIMENSION (:,:)  :: C
          INTEGER,INTENT(IN)                            :: N1,N2,N3,N4
        END SUBROUTINE KRON
        SUBROUTINE CUTM (PP,NONR,NT,NA,NOR)
          IMPLICIT NONE
          DOUBLE PRECISION,INTENT(INOUT),DIMENSION (:,:) :: PP
          INTEGER,INTENT(IN),DIMENSION (:,:)             :: NONR
          INTEGER,INTENT(IN)                             :: NT,NA
          INTEGER,INTENT(OUT)                            :: NOR
        END SUBROUTINE CUTM
        SUBROUTINE INVMTX (N,A,B)
          INTEGER,INTENT(IN)                             :: N
          DOUBLE PRECISION,INTENT(IN), DIMENSION (:,:)   :: A
          DOUBLE PRECISION,INTENT(OUT),DIMENSION (:,:)   :: B
        END SUBROUTINE INVMTX
        SUBROUTINE ADDM (PPI,NONR,NT,NA,NR)
          IMPLICIT NONE
          DOUBLE PRECISION,INTENT(INOUT),DIMENSION (:,:) :: PPI
          INTEGER,INTENT(IN),DIMENSION (:,:)             :: NONR
          INTEGER,INTENT(IN)                             :: NT,NA,NR
        END SUBROUTINE ADDM
        SUBROUTINE SPDCGG (N,A,B,X)
          IMPLICIT NONE
          INTEGER,INTENT(IN)                             :: N
          DOUBLE PRECISION,INTENT(IN),DIMENSION (:,:)    :: A
          DOUBLE PRECISION,INTENT(IN),DIMENSION (:)      :: B
          DOUBLE PRECISION,INTENT(OUT),DIMENSION (:)     :: X
        END SUBROUTINE SPDCGG
      END INTERFACE
      CALL DATE_AND_TIME (DATE,TIME)
      WRITE (6,'(A7,A8,2X,A7,A8)') 'Date = ',DATE,'Time = ',TIME
      WRITE (6,*)
      OPEN (11,FILE='GParameter.csv')
      NT = 0
   10 READ (11,*,END=20)
      NT = NT+1
      GO TO 10
   20 REWIND (11)
      ALLOCATE (GP(NT,NT),K0(NT),G0(NT,NT),R0(NT,NT),P0(NT,NT),WVP(NT))
      ALLOCATE (P0I(NT,NT),GPG0(NT,NT),GPG0I(NT,NT))
      DO I=1,NT
        READ (11,*) K0(I),WVP(I),GP(I,1:NT)
      END DO
      CLOSE (11)
      DO I=1,NT
        DO J=1,NT
          IF (I.EQ.J) THEN
            G0(I,I) = GP(I,I); R0(I,I) = WVP(I)-G0(I,I)
          ELSE IF (I.GE.J) THEN
            G0(I,J) = GP(I,J); G0(J,I) = G0(I,J)
          ELSE
            R0(I,J) = GP(I,J); R0(J,I) = R0(I,J)
          END IF
        END DO
      END DO
      P0 = G0+R0
      CALL INVMTX (NT,P0,P0I)
      OPEN (11,FILE='Pedigree&Record.csv')
      NA = 0
   30 READ (11,*,END=40)
      NA = NA+1
      GO TO 30
   40 REWIND (11)
      ALLOCATE (ID1(NA),ID2(NA),CAND(NA),GEND(NA),NONR(NA,NT))
      ALLOCATE (A0(NA,NA),PP(NT*NA,NT*NA),PPI(NT*NA,NT*NA),GA0(NT*NA,NT))
      ALLOCATE (LHS(NT,NT),RHS(NT),V0(NT),VV0(NT),VV1(NT),VV2(NT))
      DO I=1,NA
        READ (11,*) IWK,ID1(I),ID2(I),CAND(I),GEND(I),NONR(I,1:NT)
      END DO
      IWK = IWK
      CLOSE (11)
      CALL AMRX (ID1,ID2,A0,NA)
      CALL KRON (G0,A0,PP,NT,NT,NA,NA)
      DO I=1,NT
        DO J=1,NT
          DO K=1,NA
            PP((I-1)*NA+K,(J-1)*NA+K) = PP((I-1)*NA+K,(J-1)*NA+K)+R0(I,J)
          END DO
        END DO
      END DO
      CALL CUTM (PP,NONR,NT,NA,NR)
      PPI = 0.0
      CALL INVMTX (NR,PP,PPI)
      CALL ADDM (PPI,NONR,NT,NA,NR)
      NS1 = 0; NS2 = 0;VV1 = 0;VV2 = 0
      GA0 = 0.0
      DO II=1,NA
        IF (CAND(II).EQ.1) THEN
          DO I=1,NT
            DO JJ=1,NT
              DO J=1,NA
                K = (I-1)*NA+J
                GA0(K,JJ) = G0(I,JJ)*A0(J,II)
              END DO
            END DO
          END DO
          LHS = MATMUL(MATMUL(TRANSPOSE(GA0),PPI),GA0)
          RHS = K0
          CALL SPDCGG (NT,LHS,RHS,VV0)
          IF (GEND(II).EQ.1) THEN
            VV1 = VV1+VV0
            NS1 = NS1+1
          ELSE
            VV2 = VV2+VV0
            NS2 = NS2+1
          END IF
        END IF
      END DO
      VV1 = VV1/NS1
      VV2 = VV2/NS2
      VV0 = (VV1+VV2)/2
      GPG0 = MATMUL(MATMUL(TRANSPOSE(G0),P0I),G0)
      CALL INVMTX (NT,GPG0,GPG0I)
      V0 = MATMUL(GPG0I,K0)
      DO I=1,NT
        IF(VV0(I).NE.0.0.AND.V0(I).NE.0.0) THEN
           RATIO = V0(I)/VV0(I)
           V0 = V0/RATIO
           EXIT
        END IF
      END DO
      WRITE (6,'(A,I8)') 'No. of animals = ',NA
      WRITE (6,'(A,I8)') 'No. of records = ',NR
      WRITE (6,*)
      OPEN (21,FILE='Weighted_value.csv')
      DO I=1,NT
        WRITE (6,'(4F12.5)') VV1(I),VV2(I),VV0(I),V0(I)
        WRITE (21,*) VV1(I),VV2(I),VV0(I),V0(I)
      END DO
      CLOSE (21)
      WRITE (6,*)
      CALL DATE_AND_TIME (DATE,TIME)
      WRITE (6,'(A7,A8,2X,A7,A8)') 'Date = ',DATE,'Time = ',TIME
      END
      SUBROUTINE KRON (A,B,C,N1,N2,N3,N4)
      IMPLICIT NONE
      DOUBLE PRECISION,INTENT(IN),DIMENSION (:,:)   :: A,B
      DOUBLE PRECISION,INTENT(OUT),DIMENSION (:,:)  :: C
      INTEGER,INTENT(IN)                            :: N1,N2,N3,N4
      INTEGER                                       :: I,J,I1,J1,I2,J2
      DO I1=1,N1
        DO J1=1,N2
          DO I2=1,N3
            DO J2=1,N4
              I = (I1-1)*N3+I2
              J = (J1-1)*N4+J2
              C(I,J) = A(I1,J1)*B(I2,J2)
            END DO
          END DO
        END DO
      END DO
      END SUBROUTINE
      SUBROUTINE AMRX (ID1,ID2,A,NA)
      IMPLICIT NONE
      INTEGER,INTENT(IN),DIMENSION (:)               :: ID1,ID2
      DOUBLE PRECISION,INTENT(OUT),DIMENSION (:,:)   :: A
      INTEGER,INTENT(IN)                             :: NA
      INTEGER                                        :: NB,BASE1,BASE2
      INTEGER                                        :: I
      DO I=1,NA
        IF (ID1(I).NE.0) THEN
          BASE1 = I-1
          EXIT
        END IF
      END DO
      DO I=1,NA
        IF (ID2(I).NE.0) THEN
          BASE2 = I-1
          EXIT
        END IF
      END DO
      NB = MIN(BASE1,BASE2)
      A = 0
      DO I=1,NA
        A(I,I) = 1
      END DO
      DO I=NB+1,NA
        IF (ID1(I).EQ.0) THEN
          A(I,1:I-1) = A(ID2(I),1:I-1)/2
          A(1:I-1,I) = A(I,1:I-1)
        ELSE IF (ID2(I).EQ.0) THEN
          A(I,1:I-1) = A(ID1(I),1:I-1)/2
          A(1:I-1,I) = A(I,1:I-1)
        ELSE
          A(I,I) = 1+A(ID1(I),ID2(I))/2
          A(I,1:I-1) = (A(ID1(I),1:I-1)+A(ID2(I),1:I-1))/2
          A(1:I-1,I) = A(I,1:I-1)
        END IF
      END DO
      RETURN
      END
      SUBROUTINE CUTM (PP,NONR,NT,NA,NR)
      IMPLICIT NONE
      DOUBLE PRECISION,INTENT(INOUT),DIMENSION (:,:) :: PP
      INTEGER,INTENT(IN),DIMENSION (:,:)             :: NONR
      INTEGER,INTENT(IN)                             :: NT,NA
      INTEGER,INTENT(OUT)                            :: NR
      INTEGER                                        :: I,J,K
      NR = 0
      DO I=NT,1,-1
        DO J=NA,1,-1
          IF (NONR(J,I).EQ.1) THEN
            NR =  NR+1
          ELSE
            DO K=(I-1)*NA+J,(I-1)*NA+J+NR-1
              PP(:,K) = PP(:,K+1)
              PP(K,:) = PP(K+1,:)
            END DO
          END IF
        END DO
      END DO
      PP(NR+1:NA,NR+1:NA) = 0.0
      RETURN
      END
      SUBROUTINE ADDM (PPI,NONR,NT,NA,NR)
      IMPLICIT NONE
      DOUBLE PRECISION,INTENT(INOUT),DIMENSION (:,:) :: PPI
      INTEGER,INTENT(IN),DIMENSION (:,:)             :: NONR
      INTEGER,INTENT(IN)                             :: NT,NA,NR
      INTEGER                                        :: I,J,K,WNR
      WNR = NR
      DO I=1,NT
        DO J=1,NA
          IF (NONR(J,I).EQ.1) THEN
            WNR =  WNR-1
          ELSE
            DO K=(I-1)*NA+J+WNR-1,(I-1)*NA+J,-1
              PPI(:,K+1) = PPI(:,K)
              PPI(K+1,:) = PPI(K,:)
            END DO
            PPI(:,(I-1)*NA+J) = 0.0
            PPI((I-1)*NA+J,:) = 0.0
          END IF
        END DO
      END DO
      RETURN
      END
      SUBROUTINE INVMTX (N,A,B)
      IMPLICIT NONE
      INTEGER,INTENT(IN)                           :: N
      DOUBLE PRECISION,INTENT(IN), DIMENSION (:,:) :: A
      DOUBLE PRECISION,INTENT(OUT),DIMENSION (:,:) :: B
      INTEGER                                      :: I,J,K
      DOUBLE PRECISION                             :: DWK
      INTEGER                                      :: MAXC
      INTEGER                                      :: IWK
      INTEGER,DIMENSION (N)                        :: WMAX
      DOUBLE PRECISION,PARAMETER                   :: EPS=1.0E-12
      IF (N.LT.1) THEN
        WRITE (6,*) 'Order of matrix is less than 1 !'
        STOP
      ELSE IF (N.EQ.1) THEN
        IF (DABS(A(1,1)).LT.EPS) THEN
          WRITE (6,*) 'Matrix is singular !'
          STOP
        END IF
        B(1,1) = 1.0/A(1,1)
        RETURN
      END IF
      B = A
      DO K=1,N
        WMAX(K) = K
      END DO
      DO K=1,N
        DWK = 0.0
        DO I=K,N
          IF (DWK.LT.DABS(B(I,1))) THEN
            DWK = DABS(B(I,1))
            MAXC = I
          END IF
        END DO
        IF (DWK.LT.EPS) THEN
          WRITE (6,*) 'Matrix is singular !'
          STOP
        END IF
        IWK = WMAX(MAXC)
        WMAX(MAXC) = WMAX(K)
        WMAX(K) = IWK
        DO J=1,N
          DWK = B(MAXC,J)
          B(MAXC,J) = B(K,J)
          B(K,J) = DWK
        END DO
        DWK = B(K,1)
        DO J=2,N
          B(K,J-1) = B(K,J)/DWK
        END DO
        B(K,N) = 1.0/DWK
        DO I=1,N
          IF (I.NE.K) THEN
            DWK = B(I,1)
            DO J=2,N
              B(I,J-1) = B(I,J)-DWK*B(K,J-1)
            END DO
            B(I,N) = -DWK*B(K,N)
          END IF
        END DO
        IF (MOD(K,100).EQ.0) THEN
          WRITE (6,*) K
        END IF
      END DO
      DO K=1,N
        DO J=K,N
          IF (WMAX(J).EQ.K) THEN
            WMAX(J) = WMAX(K)
            DO I=1,N
              DWK = B(I,J)
              B(I,J) = B(I,K)
              B(I,K) = DWK
            END DO
            GO TO 10
          END IF
        END DO
   10 END DO
      RETURN
      END SUBROUTINE INVMTX
      SUBROUTINE SPDCGG (N,A,B,X)
      IMPLICIT NONE
      INTEGER,INTENT(IN)                                :: N                  ! Rank
      DOUBLE PRECISION,INTENT(IN),DIMENSION (:,:)       :: A                  ! LHS
      DOUBLE PRECISION,INTENT(IN),DIMENSION (:)         :: B                  ! RHS
      DOUBLE PRECISION,INTENT(OUT),DIMENSION (:)        :: X                  ! Solution
      DOUBLE PRECISION,DIMENSION (N)                    :: P,R,AP,WX	
      DOUBLE PRECISION                                  :: ALPHA,BETA,PAP
       DOUBLE PRECISION                                 :: X2,ED2,ERR
      INTEGER                                           :: RPT
      INTEGER                                           :: I
      DOUBLE PRECISION,PARAMETER                        :: EPS=1.0D-12  ! 逆行列の精度
      RPT = 0
      P = B; R = P
      X = 0.0
  100 WX = X
      AP = MATMUL(A,P)
      PAP = DOT_PRODUCT(P,AP)
      ALPHA = DOT_PRODUCT(P,R)/PAP
      X = X+ALPHA*P
      R = R-ALPHA*AP
      BETA = -DOT_PRODUCT(R,AP)/PAP
      P = R+BETA*P
      ED2 = 0; X2 = 0
      DO I=1,N
        ED2 = ED2+(X(I)-WX(I))**2
        X2 = X2+X(I)**2
      END DO
      ERR = DSQRT(ED2/X2)
      RPT = RPT+1
      IF (RPT.EQ.N*100) THEN
        WRITE (6,'(A,I10)') 'No. iterations = ',N*100; STOP
      END IF
      IF (ERR.GT.EPS)     GO TO 100
      RETURN
      END SUBROUTINE SPDCGG
