      program mtjaam3
c-----------------------------------------------------------------------
c Program MTJAAM version 3.7
c
c Copyright by Nicolas Gengler 
c
c This version forces last genetic group solutions to zero 10/15/97
c
c PEV correction done 12/23/96
c Long filename correction done 02/19/97
c Correction avoiding wrong number of missing value patterns 11/17/97
c Minor adjustments for PEV avoiding problems with un-related
c animals without data records in pedigree file 03/17/98
c bug fixed, regressions work now with missing values 09/11/98
c
c-----------------------------------------------------------------------
c
      integer mxhash
      integer mx,ntrait,nrand,irew,iopen,iwrite,iread,kkn,kk,k1,j1
     $ ,kn,j2,isr,ian,itype,idam,nr,irelat,isize,iposmx,nfix,irand,i
     $ ,imax,nooffa,nunout,nunin,itrno,itr,mt,nt,jj,n1,kkk,ii,miss
     $ ,imiss,ipev,n,k,j,ldprior,icut,npev,nk,i1
      parameter(mxhash=50000)
      parameter (mx=200000,ntrait=16,nrand=4)
      parameter(irew=1,iopen=2,iwrite=3,iread=4)

      real sol(mx,ntrait),trsol(ntrait),g(ntrait,ntrait,nrand)
      real gi(ntrait,ntrait)
      real*8 gg(ntrait,ntrait,nrand)
      real acc(mx),diag(mx),dsol(mx,ntrait)
*------------inbreeding----------------------------------------
      integer inb(mx),inbrs,inbrd,idinbr,inbred,in,ninb
*--------------------------------------------------------------
      real r(ntrait,ntrait),varw(ntrait,nrand)
      real*8 rr(ntrait,ntrait),vvarw(ntrait,nrand)
      real w(ntrait,ntrait),winv(ntrait,ntrait)
      real*8 ww(ntrait,ntrait),wwinv(ntrait,ntrait)
      real vmiss(ntrait),can(ntrait),tt(ntrait)
      real qa(ntrait,ntrait),qb(ntrait,ntrait)
      real qia(ntrait,ntrait),qib(ntrait,ntrait)
      real aa(ntrait,ntrait),ba(ntrait,ntrait)
      real a(ntrait,ntrait),b(ntrait,ntrait),c(ntrait,ntrait)
      real q1(ntrait,ntrait,100)
      real q2(ntrait,ntrait,100)
      real var(20),buf(50),val(20)
      real reliab(ntrait)
      real var1,var2,adj1
      real adj0,adj,adjmax,y,add,rel1,conv,xx,rel,sum,type
      integer ifact(ntrait+21),ifact1(ntrait+21),ifil(ntrait+21)
     $  ,ilev(20),ilev1(20),ipos(20),level1,val1
      integer seqhash(0:mxhash),hashno(0:mxhash),kma/0/
     $   ,kmaold,hashan,hash,mvtype,seq
      integer stat,ranind(nrand)
      logical ireg(20)
      character*30 npar,ndat,nrel,nout
      character fn*6,fn1*8


*--------------storage for pev-----------------------------------
      real pevo(mx),pevn(mx),itot(3),ifra(3),elm(mx),
     $trpev(mx,ntrait),offd,weight
      real lambda(ntrait,1000)
*----------------------------------------------------------------
      print *
      print *,'Program MTJAAM Version 3.7'
      print *,'--------------------------'
      print *,'(C) by N. Gengler'
      print *
      print *
      print *,'Revision date : 11 September 1998'
      print *
      print *,'Please acknowledge its use in all publications'
      print *
      print *
c the parameters can be read either from the console or from a file
      write(*,*)'name of a file with parameters, con - if console:'
      read(*,3)npar
3     format(a30)
      if (npar .ne. 'con') then
          nunin=1
          nunout=20
          open(nunin,file=npar)
          open(nunout,file='null')
        else
          nunin=5
          nunout=6
      endif
      write(nunout,*)'data name:'
      read(nunin,3)ndat
      write(nunout,*)'name of file with animal relationships:'
      read(nunin,3)nrel
      write(nunout,*)'name of output file:'
      read(nunin,3)nout
      open(2,file=ndat)
      open(3,file=nrel)
      open(7,file=nout)
5     write(nunout,*) 'no of factors, trait pos., no of traits,',
     * ' rel. factor, iterations?:'
      read(nunin,*)nooffa,itr,itrno,rel,imax
      write(nunout,*)' for each factor give position, no of levels,'
      write(nunout,*)' and variance ratio, 0 if fixed, negative',
     * ' if relat. matrix'
      write(nunout,*)'  no of levels < 0 denotes covariable'
      write(nunout,*)
      iposmx=itr
      isize=0
      irelat=0
      if (nooffa .gt. 20) then
        print*,'Number of factors (nooffa) > 20, Increase dimensions'
        stop 83
      endif
      do 10 i=1,nooffa
        read(nunin,*)ipos(i),ilev(i),var(i)
c number of levels <0  denotes covariable
        if (ilev(i) .lt. 0) then
          ilev(i)=1
          ireg(i)=.true.
        else
          ireg(i)=.false.
        endif
        if (var(i) .ne. 0) then
          if (var(i) .lt. 0) then
            if (irelat .eq. 1) then
              print*,'only one relat. matrix allowed, type all again'
              stop
            else
              irelat=1
            endif
          endif
          irand=irand+1
          ranind(irand)=i
        else
          nfix=nfix+1
        endif
        iposmx=max(iposmx,ipos(i))
        ilev1(i)=isize
        isize=isize+ilev(i)
10    enddo
      write(nunout,'('' calculate approximate PEV (1=yes)?'')')
      read(nunin,*)ipev
      write(nunout,'('' missing values (1=yes)?'')')
      read(nunin,*)imiss
      if (imiss .eq. 1) then
        write(nunout,'('' code for missing values?'')')
        read(nunin,*)miss
      endif
*
35    if (isize .gt. mx) then
         print*,'too large model, increase parameter mx'
         stop
      endif

c read g and r if exist, create otherwise
      open(31,file='gr'//npar)
      read(31,*,iostat=stat)(((gg(i,j,k),i=1,itrno),j=1,itrno),
     +      k=1,irand),((rr(i,j),i=1,itrno),j=1,itrno)

      if (stat .eq. 0) then
          print*,'initial (co)variances read from parameters'' file'
        else
          print*,'initial (co)variances'' file',
     x' does not exist, or is not correct!!'
          print*,'please correct or create one'
          stop
      endif

c copy gg and rr to g and r (single precision)
      do k=1,irand
       do i=1,itrno
        do j=1,itrno
         g(i,j,k)=gg(i,j,k)
        enddo
       enddo
      enddo

      do i=1,itrno
       do j=1,itrno
        r(i,j)=rr(i,j)
       enddo
      enddo
c calculate coefficients of the canonical transformation
      do i=1,irand

        write(7,*)' g',i
        call prden(g(1,1,i),ntrait,itrno,7,'(6f13.5)')

      enddo
      write(7,'('' r'')')

      call prden(r,ntrait,itrno,7,'(6f13.5)')

      call tcanon1(ntrait,itrno,irand,gg,rr,ww,wwinv,vvarw)

      do i=1,itrno
        do j=1,itrno
         w(i,j)=ww(i,j)
         winv(i,j)=wwinv(i,j)
        enddo
      enddo

      do i=1,itrno
        do j=1,irand
         varw(i,j)=vvarw(i,j)
        enddo
      enddo

      print*,'variance ratios after transformation'
      print '(6g12.4)',((varw(i,j),i=1,itrno),j=1,irand)

       write(7,'('' w'')')

      call prden(w,ntrait,itrno,7,'(6f13.5)')

c       write(10,'('' w'')')

c      call prden(w,ntrait,itrno,10,'(6f13.5)')

       write(7,'('' winv'')')

      call prden(winv,ntrait,itrno,7,'(6f13.5)')

c       write(10,'('' winv'')')

c      call prden(winv,ntrait,itrno,10,'(6f13.5)')

c maximum varw is 10e5
      do  i=1,itrno
        do j=1,irand
          if (abs(varw(i,j)) .gt. .1e5) varw(i,j)=1e5
        enddo
      enddo

* Load Priors ------------------------
      write(nunout,'('' Load priors from files prior.?? (1=yes)'')')
      read(nunin,*) ldprior
      if (ldprior .eq. 1) then
        kkk=1
        write(fn1,'(''prior.'',i2.2)') kkk
        open (20+kkk,file=fn1)
        do n=1,isize
          if (n .gt. (ilev1(kkk)+ilev(kkk))) then
            close (20+kkk)
            kkk=kkk+1
            write(fn1,'(''prior.'',i2.2)') kkk
            open (20+kkk,file=fn1)
          endif
          read (20+kkk,9999)(trsol(kk),kk=1,itrno)
          do i=1,itrno
            sol(n,i)=0.
            do k=1,itrno
              sol(n,i) = sol(n,i)+ (1.e5)*w(i,k)*trsol(k)
            enddo
          enddo
        enddo
        close (20+kkk)
        do i=1,isize
          do k=1,itrno
            dsol(i,k) = sol(i,k)   ! Copy into previous solution vector
          enddo
        enddo
        print *,isize,' Priors loaded'
      endif


      do i=1,mxhash
        hashno(i) = 0    ! Zero Hash table
      enddo
      n=0
      call iof(iopen,ifil,n,4)
      call iof1(iopen,ifil,n,8)
22    read(2,*,end=24)(buf(i),i=1,iposmx+itrno-1)
      n=n+1
c     if (mod(n,10000) .eq. 0) print*,n,' Records so far'
      do i=1,nooffa
        ifil(i)= int(buf(ipos(i)))
      enddo
      mvtype=0

      do i=1,itrno
        vmiss(i)=0.
        if ((imiss.eq.1).and.(buf(itr+i-1) .eq. miss)) then
          mvtype=mvtype+2**i
          vmiss(i)=1.
        endif
      enddo
c      if (type .eq. 0.) goto 34
c       write(10,*)'Pass1'
      hash=hashan(mvtype,kma,hashno,mxhash)
c      write(10,*)n,hash,type,kma
      if (kma .gt. kmaold) then
        kmaold=kma
        seqhash(hash)=kma
c       write(10,*)'Pass2',kma

c     Q => Qa and Qb  !  Qi => Qia and Qib
        do i=1,itrno
          ii=0
          jj=0
          do j=1,itrno
            if (vmiss(j) .eq. 0.) then
              ii=ii+1
              qa(i,ii)=w(i,j)
              qia(ii,i)=winv(j,i)
            else
              jj=jj+1
              qb(i,jj)=w(i,j)
              qib(jj,i)=winv(j,i)
            endif
          enddo
        enddo
        nt=ii
        mt=jj
c      call prden(w(1,1),ntrait,itrno,10,'(6f13.5)')
c      call prden(qa(1,1),ntrait,itrno,10,'(6f13.5)')
c      call prden(qb(1,1),ntrait,itrno,10,'(6f13.5)')
c      call prden(winv(1,1),ntrait,itrno,10,'(6f13.5)')
c      call prden(qia(1,1),ntrait,itrno,10,'(6f13.5)')
c      call prden(qib(1,1),ntrait,itrno,10,'(6f13.5)')

c       write(10,*)'Pass3',nt,mt

c     R => Raa
        ii=0
        do i=1,itrno
          jj=0
          if (vmiss(i) .ne. 1.) then
            ii=ii+1
            do j=1,itrno
              if (vmiss(j) .ne. 1.) then
                jj=jj+1
                aa(ii,jj)=r(i,j)
              endif
            enddo
          endif
        enddo
c      call prden(r(1,1),ntrait,itrno,10,'(6f13.5)')
c      call prden(aa(1,1),ntrait,nt,10,'(6f13.5)')

c     R => Rba
        ii=0
        do i=1,itrno
          jj=0
          if (vmiss(i) .eq. 1.) then
            ii=ii+1
            do j=1,itrno
              if (vmiss(j) .ne. 1.) then
                jj=jj+1
                ba(ii,jj)=r(i,j)
              endif
            enddo
          endif
        enddo
c      call prden(ba(1,1),ntrait,itrno,10,'(6f13.5)')

c  Q1 = Qa + Qb * Rba * inv(RAA)
        call invrt(aa,ntrait,nt)
c       write(10,*)'Pass4 after inversion'
c      call prden(aa(1,1),ntrait,nt,10,'(6f13.5)')
        do i=1,mt
          do j=1,nt
            sum=0.
            do k=1,nt
              sum=sum+ba(i,k)*aa(k,j)
            enddo
            a(i,j)=sum
          enddo
        enddo
c      call prden(a(1,1),ntrait,itrno,10,'(6f13.5)')

        do i=1,itrno
          do j=1,nt
            sum=0.
            do k=1,mt
              sum=sum+qb(i,k)*a(k,j)
            enddo
            b(i,j)=sum
          enddo
        enddo
c      call prden(b(1,1),ntrait,itrno,10,'(6f13.5)')

        do i=1,itrno
          do j=1,nt
            q1(i,j,kma)=b(i,j)+qa(i,j)
          enddo
        enddo
c      write(10,*)' q1 ',kma
c      call prden(q1(1,1,kma),ntrait,itrno,10,'(6f13.5)')

c  Q2 = Qb*Qib - Qb * Rba * inv(RAA) *Qia

        do i=1,itrno
          do j=1,itrno
            sum=0.
            do k=1,nt
              sum=sum+b(i,k)*qia(k,j)
            enddo
            c(i,j)=sum
          enddo
        enddo

c      call prden(c(1,1),ntrait,itrno,10,'(6f13.5)')

        do i=1,itrno
          do j=1,itrno
            sum=0.
            do k=1,mt
              sum=sum+qb(i,k)*qib(k,j)
            enddo
            q2(i,j,kma)=sum - c(i,j)
          enddo
        enddo

c      write(10,*)' q2 ',kma
c      call prden(q2(1,1,kma),ntrait,itrno,10,'(6f13.5)')
       if (ipev.ne.1) goto 99
c compute lambda for evey missing value pattern

       do i=1,itrno
         do j=1,itrno
           gi(i,j)=g(i,j,irand) 
         enddo
       enddo
       call prden(gi,ntrait,itrno,10,'(6f13.5)')

       call invrt(gi,ntrait,itrno)

       call prden(gi,ntrait,itrno,10,'(6f13.5)')

       call prden(aa,ntrait,nt,10,'(6f13.5)')

       ii=0
       do i=1,itrno
         jj=0
         if (vmiss(i) .ne. 1.) then
           ii=ii+1
           do j=1,itrno
             if (vmiss(j) .ne. 1.) then
               jj=jj+1
               gi(i,j)=gi(i,j)+aa(ii,jj)
             endif
           enddo
         endif
       enddo

       call prden(gi,ntrait,itrno,10,'(6f13.5)')

c compute W_j

       call invrt(gi,ntrait,itrno)

       call prden(gi,ntrait,itrno,10,'(6f13.5)')

c compute Q*W_j*Q'
  
        do i=1,itrno
          do j=1,itrno
            sum=0.
            do k=1,itrno
              sum=sum+w(i,k)*gi(k,j)
            enddo
            a(i,j)=sum
          enddo
        enddo

       call prden(a,ntrait,itrno,10,'(6f13.5)')


        do i=1,itrno
          do j=1,itrno
            sum=0.
            do k=1,itrno
              sum=sum+a(i,k)*w(j,k)
            enddo
            b(i,j)=sum
          enddo
        enddo

       call prden(b,ntrait,itrno,10,'(6f13.5)')

c check off-diagonals
        offd=0.
          do j=1,itrno
            do k=j+1,itrno
              offd=offd+abs(b(j,k)**2/b(j,j)/b(k,k))
            enddo
          enddo
        offd=sqrt(+1e-20+offd)/(itrno*(itrno-1)/2)
        print *,'Missing pattern ',kma,' Rel. off-diag. ',offd
c  compute Lambda:
        do i=1,itrno
          lambda(i,kma)=(1/b(i,i))-varw(i,irand)
        enddo     
c        print *,(lambda(i,kma),i=1,itrno)
99    endif
      
c     print *,'L348',n,hash,type,seq,kma,(nooffa+itrno+1)
      seq=seqhash(hash)

          ifil(nooffa+itrno+1)=seq
c      write(10,*)n,(vmiss(j),j=1,itrno),type,seq
      if (seq .eq. 0) then
        do i=1,itrno
          can(i)=0.
          do j=1,itrno
            can(i)=can(i)+w(i,j)*buf(itr+j-1)
          enddo
        enddo
      else
        do i=1,itrno
          jj=0
          can(i)=0.
          do j=1,itrno
            if (vmiss(j) .ne. 1.) then
              jj=jj+1
              kkk=seq
c         write(10,*)'q1 ',q1(i,jj,kkk),'buf ',buf(itr+j-1), 'i',i
c    $ ,'itr+j-1',itr+j-1
              can(i)=can(i)+q1(i,jj,kkk)*buf(itr+j-1)
            endif
          enddo
        enddo
c          ifil(nooffa+itrno+1)=int(seq)
      endif
      do i=1,itrno
        ifil(nooffa+i)=NINT(can(i)*1e5)
      enddo
      n1=nooffa+itrno+1
c      write(10,*)(ifil(kkk),kkk=1,n1)
      call iof(iwrite,ifil,n1,4)
      goto 22   ! Read next record

* ----------------- End of Read Loop --------------------

24    nr=1
      print *,kma,' Missing value patterns'
      print *,n,' Records in'
      if (irelat .eq. 1) then
27      read(3,*,end=26)(ifact(i),i=1,4)
        nr=nr+1
        call iof1(iwrite,ifact,4,8)
        goto 27
      endif


26    do i=1,mx
        inb(mx)=0
      enddo
      write(nunout,'('' Use inbreeding from inbred file (1=yes)'')')
      read(nunin,*) idinbr
      if (idinbr.eq.1) then
        open(32,file='ib'//nrel)
111     read(32,*,end=112)in,inbred
        inb(in)=inbred
          ninb=ninb+1
        goto 111
112     print *,ninb,' inbreeding values read from inbred file'
      endif

      i=1
*
25    adj=0.
      adj0=0.

      do 130 kn=1,itrno
        do ii=1,irand
          if ((var(ii+nfix)) .lt. 0.) then
            var(ii+nfix)=-varw(kn,ii)
          else
            var(ii+nfix)=varw(kn,ii)
          endif
        enddo
        do j=1,isize
          acc(j)=0
          diag(j)=0
        enddo
* do one round of gauss-seidel (jacobi if relationship matrix)
        do 110 jj=1,nooffa
          call iof(irew,ifact,0,4)
          do 60 ii=1,n
            n1=nooffa+itrno+1
            call iof(iread,ifact1,n1,4)
c      write(10,*)(ifact1(kkk),kkk=1,n1)
c      print *,'L422',n1,ii,n
            kma=ifact1(n1)
c      if (i .eq. 1) write(10,*)ii,kma
            if (ifact1(n1) .eq. 0) then
              y=float(ifact1(nooffa+kn))
c      write(10,*)'y',y
            else
              y=float(ifact1(nooffa+kn))
              do kkk=1,itrno
                tt(kkk)=0.
                do kk=1,nooffa
                  if (ireg(kk)) then
                    val1=ifact1(kk)
                    level1=1
                  else
                    val1=1
                    level1=ifact1(kk)
                  endif
                  kkn=ilev1(kk)+level1   
                  tt(kkk)=tt(kkk)+val1*sol(kkn,kkk)
                enddo
              enddo
c               write(10,*)(tt(kkk),kkk=1,itrno)
              do kkk=1,itrno
                y=y+q2(kn,kkk,kma)*tt(kkk)
c           write(10,*)'y ',y,'q2 ',q2(kn,kkk,kma),'tt ',tt(kkk)
              enddo
            endif
            do j=1,nooffa
              if (ireg(j)) then
                val(j)=ifact1(j)
                ifact1(j)=1
              else
                if ((ifact1(j).le.ilev(j)).and.(ifact1(j).ge.1))then
                  val(j)=1
                else
                  val(j)=0
                endif
              endif
              ifact(j)=ifact1(j)+ilev1(j)
            enddo
            j1=ifact(jj)
            diag(ifact(jj))=diag(ifact(jj))+val(jj)*val(jj)
            acc(ifact(jj))=acc(ifact(jj))-y*val(jj)
            do k=1,nooffa
              k1=ifact(k)
              if (k1 .ne. j1) acc(ifact(jj))=acc(ifact(jj))+
     +                         sol(k1,kn)*val(jj)*val(k)
            enddo
60        enddo

* solve each factor separately, only fixed factors during the first
* round
          j1=ilev1(jj)+1
          j2=ilev(jj)+ilev1(jj)

* add variance ratio to diagonals of random factors
          if (var(jj) .gt. 0) then
            do k=j1,j2
              diag(k)=diag(k)+var(jj)
            enddo
          endif

          if (var(jj) .lt. 0.) then
* add animal relationship matrix if such exists
            call iof1(irew,ifact,0,8)
            do k=1,nr-1
              call iof1(iread,ifact,4,8)
              ian=ifact(1)
              isr=ifact(2)
              idam=ifact(3)
              itype=ifact(4)
              inbrs=inb(isr)
              inbrd=inb(idam)
*              if (itype .eq. 1) type=-var(jj)
*              if (itype .eq. 2) type=-var(jj)*2./3
*              if (itype .eq. 3) type=-var(jj)*.5
              if (itype .eq. 1) 
     $          type=-var(jj)*2./(2.-.001*(inbrs+inbrd))
              if (itype .eq. 2) then
                if (isr.gt.idam) then
                  type=-var(jj)*2./(3.-.001*(2*inbrs+inbrd))
                else if (isr.lt.idam) then
                  type=-var(jj)*2./(3.-.001*(inbrs+2*inbrd))
                endif
              endif
              if (itype .eq. 3) 
     $            type=-var(jj)*1./(2.-.001*(inbrs+inbrd))
              ian=ian+ilev1(jj)
              isr=isr+ilev1(jj)
              idam=idam+ilev1(jj)
              diag(ian)=diag(ian)+2*type
              diag(idam)=diag(idam)+type*0.5
              diag(isr)=diag(isr)+type*0.5
              acc(ian)=acc(ian)-type*(sol(isr,kn)+sol(idam,kn))
              acc(isr)=acc(isr)-type*(sol(ian,kn)-.5*sol(idam,kn))
              acc(idam)=acc(idam)-type*(sol(ian,kn)-.5*sol(isr,kn))
            enddo
          endif
* solve
          do k=j1,j2
            rel1=0
            if (var(jj) .lt. 0) rel1=rel
            if (diag(k) .ne. 0) then
              add=-acc(k)/diag(k)-sol(k,kn)
              adj=adj+add*add
              xx=sol(k,kn)
              sol(k,kn)=sol(k,kn)+rel1*(sol(k,kn)-dsol(k,kn))+add
c       sol(k,kn)=sol(k,kn)+add
C**************************************
C       Last genetic group solution forced to zero
C
          if ((k.eq.j2).and.(var(jj).lt.0)) then
            sol(k,kn)=0
            xx=0
          endif
C**************************************          
              dsol(k,kn)=xx
              adj0=adj0+sol(k,kn)*sol(k,kn)
c       write(10,*)adj,adj0,sol(k,kn),dsol(k,kn)
            endif
          enddo
110     enddo
130   enddo
      conv=adj/adj0
      write(6,*)i,conv
      i=i+1
      if (i .lt. imax ) goto 25
      write(*,135)
135   format(' how many more iterations, 0-none?')
      read(*,*)j
      if (j .gt. 0) then
      imax=imax+j
      goto 25
      endif

                                                      
*
* +++++++++++++   p e v begins here ++++++++++++++++++
      if ((var(nooffa).ge.0).or.(ipev.ne.1)) goto 500
      print 200
200   format(' In approximate Prediction Error Variance for animals',
     +       ' it will be assumed that:'/
     +        '  first effect is fixed like HYS'/
     +        '  the last effect is animal')
      if (var(nooffa-1).ne.0) print*,'  effect before the last is',
     +     ' permanent environment'


      icut=ilev(nooffa)-nr+1
      print '('' detected'',i8,'' unknown parent group(s)'')',icut
      write(*,'(/10x,'' Calculation of approximate PEV''/)')
      write(7,'(/10x,'' Calculation of approximate PEV''/)')
c      write(*,'('' iteration no. convergence PEV(1)    PEV(2) ....'')')
c      write(7,'('' iteration no. convergence PEV(1)    PEV(2) ....'')')
      
c initialize

      do kn=1,itrno
      print *,'PEV for canonical trait ',kn
      npev=1
      var1=varw(kn,(irand-1))
      var2=abs(varw(kn,irand))
      do 350 i=1,isize
        elm(i)=0
        diag(i)=0
        pevn(i)=0
350     pevo(i)=0


        call iof(irew,ifact,0,4)
        do ii=1,n
          n1=nooffa+itrno+1
          call iof(iread,ifact1,n1,4)
          kma=ifact1(n1)
          if (ifact1(n1) .eq. 0) then
            weight=1.
          else
            weight=lambda(kn,kma)
          endif
          ifact(1)=ifact1(1)+ilev1(1)
          diag(ifact(1))=diag(ifact(1))+weight
        enddo  
c calculate and add effective lactations to new pev
c animal effects is assumed last on the list (nooffa)
c
        call iof(irew,ifact,0,4)
      do 430 ii=1,n
        n1=nooffa+itrno+1
        call iof(iread,ifact,n1,4)
        kma=ifact(n1)
        if (ifact(n1) .eq. 0) then
          weight=1.
        else
          weight=lambda(kn,kma)
        endif

        do 440 j=2,nooffa-1
440        ifact(j)=ifact(j)+ilev1(j)
           i1=ifact(nooffa)
           weight=max(0.,weight*(1-weight/diag(ifact(1))))
           weight=min(weight,1.)
           elm(i1)=elm(i1)+weight
430   continue
c
c If no permanent environment effect present, comment lines below
c
      if (var1.ne.0.0) then
       do 460 i=1,ilev(nooffa)-icut
           elm(i)=elm(i)*var1/(elm(i)+var1)
460      continue
      endif
c end of permanent environment section

            do 465 i=1,isize
465           pevo(i)=elm(i)

c start pev loop here
467    do 468 i=1,isize
468       pevn(i)=elm(i)
c calculate contributions due to relationships
      call iof1(irew,ifact,0,8)
      do 470 ii=1,nr-1
           call iof1(iread,ifact,4,8)
           ian=ifact(1)
           isr=ifact(2)
           idam=ifact(3)
           if (idam.le.ilev(nooffa)-icut .and. idam.ne.0) then
                          itot(1)=pevo(idam)
             else
               itot(1)=0
           endif
           if (isr.le.ilev(nooffa)-icut .and. isr.ne.0) then
               itot(2)=pevo(isr)
             else
               itot(2)=0
           endif
           itot(3)=pevo(ian)
           call deduct(itot,ifra,var2)
c          print '(3i3,9f10.6)',idam,isr,ian,itot,ifra
           if (idam.le.ilev(nooffa)-icut .and. idam.ne.0)
     $                             pevn(idam)=pevn(idam)+ifra(1)
           if (isr.le.ilev(nooffa)-icut .and. isr.ne.0)
     $                             pevn(isr)=pevn(isr)+ifra(2)
           pevn(ian)=pevn(ian)+ifra(3)
470   continue

      adj=0.
      adjmax=0.
      adj1=0.
      npev=npev+1
      do 480 i=1,ilev(nooffa)-icut
        adj=adj+abs(pevn(i)-pevo(i))
        adj1=adj1+abs(pevn(i))
c480     pevo(i)=pevn(i)
480     pevo(i)=(pevn(i)+pevo(i))/2
       xx=adj/adj1
       write(*,*)npev-1,xx
       if (npev.le.20) then
          if (xx.gt.1e-4) goto 467
       endif
      do i=1,nr
        do k=1,itrno
        if (pevn(i).gt.0.) then
          xx=1./(pevn(i)+var2)
c Animals without records and un-related is given maximum PEV
        else if (elm(i).eq.0) then
          xx=1./var2
        else 
          xx=0.
        endif
        trpev(i,k)=trpev(i,k)+(winv(k,kn)**2)*xx
        enddo
      enddo
      enddo
500   kkk=1
      write(fn,'(''sol.'',i2.2)') kkk
      open (20+kkk,file=fn)
      do n=1,isize
        if (n .gt. (ilev1(kkk)+ilev(kkk))) then
          close (20+kkk)
          kkk=kkk+1
          write(fn,'(''sol.'',i2.2)') kkk
          open (20+kkk,file=fn)
        endif
        do i=1,itrno
          trsol(i)=0.
          do k=1,itrno
            trsol(i)=trsol(i)+winv(i,k)*sol(n,k)
          enddo
          trsol(i)=trsol(i)/1e5
        enddo
        if ((ipev.eq.1).and.(kkk.eq.nooffa)) then
          nk=n-ilev1(nooffa)
          do k=1,itrno
            if (trpev(nk,k).ne.0.) then
              trpev(nk,k) = min(trpev(nk,k),g(k,k,irand))
              reliab(k)=1-(trpev(nk,k)/g(k,k,irand))
            else
              reliab(k)=0.
            endif
          enddo
          write(20+kkk,9999)(trsol(kk),kk=1,itrno),
     $                      (reliab(kk),kk=1,itrno)
        else
          write(20+kkk,9999)(trsol(kk),kk=1,itrno)
        endif
      enddo
9999  format(<itrno>f8.2,<itrno>f6.2)
      print *,'solutions were stored in files sol.01 - ',fn

      stop
      end



      subroutine deduct(b,g,al)
c from total information on animal and its parents, calculates fraction
c due to this particular relationships.
c  b - on input contains total information for parent 1, parent 2, and animal
c  g - on output contains fraction f the total information due to this 
c      relationship
c  al - variance ratio
c
c unknown parent has information = 0.
*
      real b(3),g(3),al,adj,q(3),s(3),bg(3)
      real al2d,al2p,al2p4d,al3p,al3m2d,al2m
      integer i,j,n
*
      n=1
      al2p=al*al
      al2p4d=al2p/4
      al3p=al2p*al
      al3m2d=3*al/2
      al2m=2*al
      al2d=al/2
*
      do 10 i=1,3
10       q(i)=b(i)
*
15    bg(1)=al2d+q(1)-(al2p4d*(al2m+q(3))-al3p+al2p*(al3m2d+q(2)))/
     *  ((al3m2d+q(2))*(al2m+q(3))-al2p)

      bg(2)=al2d+q(2)-(al2p4d*(al2m+q(3))-al3p+al2p*(al3m2d+q(1)))/
     *  ((al3m2d+q(1))*(al2m+q(3))-al2p)

      bg(3)=al+q(3)-(al2p*(al3m2d+q(2))-al3p+al2p*(al3m2d+q(1)))/
     *  ((al3m2d+q(1))*(al3m2d+q(2))-al2p4d)
*
      adj=0
      do 20 j=1,3
        if (b(j) .eq. 0) then
            s(j)=0
          else
            s(j)=bg(j)-b(j)
        endif
        adj=adj+s(j)*s(j)
        q(j)=q(j)-s(j)
        if (q(j) .lt. 0) q(j)=0
        g(j)=bg(j)-q(j)
20    continue
*
      n=n+1
      if ((n .lt. 10).and.(adj .gt. 10e-10)) goto 15
*
      end



      subroutine iof(itype,vector,n,iun)
c I/O facility to read and write vectors of any size using
c large buffer, thus increasing unformatted i/o speed.
c
c itype  - operation type, as in the parameter statement below,
c vector - contains data to be stored or retrieved,
c      n - size of vector,
c    iun - unit number.
c
c If the data is small enough to fit in the buffer, subsequent reads
c are done entirely in memory with a RAM speed. Option "iopen" creates
c a temporary file. Definition of a named file should be done explicitly
c in a user program, followed by a call to this subroutine with "irew"
c option.
c
c For greater speed the subroutine should be used with one file only.
c More copies of it with different names (iof1,iof2...) can be used if
c more files are desired.
c
      integer irew,iopen,iwrite,iread,bufs,n,fpos,bpos,iwr,lastit
     $  ,n1,i,iun,itype
      parameter(irew=1,iopen=2,iwrite=3,iread=4)
*     implicit integer (b,f,i)
      parameter(bufs=1024)
      integer x(bufs),vector(n)
      save x,fpos,bpos,iwr,lastit
      data fpos,bpos/2*-1/
***********************************************************************
      if(itype .eq. iread) then
        iwr=0
        if (fpos .eq. 0) then
          read(iun,end=50)x
          fpos=1
          bpos=0
        endif
        n1=min(n,bufs-bpos)
        do 30 i=1,n1
30        vector(i)=x(bpos+i)
        bpos=bpos+n1
        if ((bpos .eq. bufs).and.(n1 .ne. n)) then
          read(iun,end=50)x
          fpos=fpos+1
          bpos=n-n1
          do 40 i=1,bpos
40          vector(i+n1)=x(i)
          endif
*
      else if(itype .eq. iwrite) then
        if (lastit .eq. irew) then
          fpos=0
          rewind iun
        endif
        iwr=1
        n1=min(n,bufs-bpos)
        do 10 i=1,n1
10      x(bpos+i)=vector(i)
        bpos=bpos+n1
        if (bpos .eq. bufs) then
          write(iun)x
          fpos=fpos+1
          bpos=n-n1
          do 20 i=1,bpos
20          x(i)=vector(i+n1)
        endif
*
      elseif (itype .eq. iopen) then
        bpos=0
        fpos=0
        open(iun,access='sequential',form='unformatted')
*
      else if (itype .eq. irew) then
        if ((bpos .ne. 0).and.(iwr .eq. 1)) then
          write(iun)x
          fpos=fpos+1
        endif
        iwr=0
        bpos=0
        if (fpos .ne. 1) then
          fpos=0
          rewind iun
        endif
*
      else
        write(*,*)'unknown i/o code=',itype
        stop
      endif
***********************************************************************
      lastit=itype
      return
50    write(*,*)'end of file',iun
      write(*,*)'bpos,fpos,n',bpos,fpos,n
      stop
      end

      subroutine iof1(itype,vector,n,iun)
      integer irew,iopen,iwrite,iread,bufs,iwr,lastit
     $  ,bpos,i,n1,itype,fpos,iun,n
      parameter(irew=1,iopen=2,iwrite=3,iread=4)
*     implicit integer (b,f,i)
      parameter(bufs=1024)
      integer x(bufs),vector(n)
      save x,fpos,bpos,iwr,lastit
      data fpos,bpos/2*-1/
***********************************************************************
      if(itype .eq. iread) then
        iwr=0
        if (fpos .eq. 0) then
          read(iun,end=50)x
          fpos=1
          bpos=0
        endif
        n1=min(n,bufs-bpos)
        do 30 i=1,n1
30        vector(i)=x(bpos+i)
        bpos=bpos+n1
        if ((bpos .eq. bufs).and.(n1 .ne. n)) then
          read(iun,end=50)x
          fpos=fpos+1
          bpos=n-n1
          do 40 i=1,bpos
40          vector(i+n1)=x(i)
          endif
*
      else if(itype .eq. iwrite) then
        if (lastit .eq. irew) then
          fpos=0
          rewind iun
        endif
        iwr=1
        n1=min(n,bufs-bpos)
        do 10 i=1,n1
10      x(bpos+i)=vector(i)
        bpos=bpos+n1
        if (bpos .eq. bufs) then
          write(iun)x
          fpos=fpos+1
          bpos=n-n1
          do 20 i=1,bpos
20          x(i)=vector(i+n1)
        endif
*
      elseif (itype .eq. iopen) then
        bpos=0
        fpos=0
        open(iun,access='sequential',form='unformatted')
*
      else if (itype .eq. irew) then
        if ((bpos .ne. 0).and.(iwr .eq. 1)) then
          write(iun)x
          fpos=fpos+1
        endif
        iwr=0
        bpos=0
        if (fpos .ne. 1) then
          fpos=0
          rewind iun
        endif
*
      else
        write(*,*)'unknown i/o code=',itype
        stop
      endif
***********************************************************************
      lastit=itype
      return
50    write(*,*)'end of file',iun
      write(*,*)'bpos,fpos,n',bpos,fpos,n
      stop
      end



      subroutine tcanon1(ntrait,itrno,irand,G,R,W,WINV,varw)
c diagonalizes R, and attempts to simultaneously diagonalize nrand
c matrices in G
      integer ntrait,itrno,irand,i,j,k,l,m,nrot,nscr,flag,nsweep,msglev
      parameter (nscr=20)
      real*8 g(ntrait,ntrait,irand),r(ntrait,ntrait),
     + winv(ntrait,ntrait),w(ntrait,ntrait),g1(nscr,nscr,5),
     + r1(nscr,nscr),x(nscr,nscr),y(20,20,5),z(nscr,nscr),d(nscr),
     + varw(ntrait,irand),weight(5),scr(nscr),d1(nscr)
      data weight/5*1d0/,msglev/0/


c bypass procedures for single trait
      if (itrno .eq. 1) then
          winv(1,1)=sqrt(r(1,1))
          w(1,1)=1/winv(1,1)
          do i=1,irand
            varw(1,i)=r(1,1)/g(1,1,i)
          enddo
          return
       endif

c      numbers 20 and 5 are coded in subroutine FG
      if ((nscr.lt.itrno).or.(irand.gt.5).or.(nscr.ne.20)) then
            print*,'nscr too low or too many random effects'
            stop
      endif

c zero and copy matrices
      do i=1,itrno
         do j=1,itrno
            winv(j,i)=0
            w(i,j)=0
            r1(j,i)=r(j,i)
            do k=1,irand
               g1(i,j,k)=g(i,j,k)
               y(i,j,k)=0
            enddo
         enddo
      enddo

      if (msglev .ge. 2) then
         print*,'r and g'
         call prden8(r,ntrait,itrno,6,'(20g10.2)')
         do i=1,irand
         call prden8(g(1,1,i),ntrait,itrno,6,'(20g10.2)')
         enddo
         print*,'d'
         print '(5g10.2)',(d(i),i=1,itrno)
      endif

c check for positive definitness and zero negative eigenvalues, if needed
      do i=1,irand
c       find e-values of G, to be able to check it is positive-definite

        call jacobi(g1(1,1,i),itrno,nscr,d,x,nrot)
        call posdef(g1(1,1,i),itrno,nscr,d,x,flag)
        if (flag .gt. 0) then
           print*,'g(',i,') not positive definite'
        endif
      enddo

c find e-values of R
      call jacobi(r1,itrno,nscr,d,x,nrot)
c check for positive definitness  and zero negative eigenvalues, if needed
      call posdef(r1,itrno,nscr,d,x,flag)
      if (flag .gt. 0) then
         print*,'r not positive definite'
      endif

c y()=sqrt(inv(d))*X'*G()*X*sqrt(inv(d))
      do i=1,itrno
         do m=1,irand
           do l=i,itrno
             do j=1,itrno
               do k=1,itrno
                 y(i,l,m)=y(i,l,m)+1/sqrt(d(i))*x(j,i)*g(j,k,m)*x(k,l)/
     +                                                   sqrt(d(l))
               enddo
             enddo
             y(l,i,m)=y(i,l,m)
           enddo
         enddo
       enddo

      if (msglev .ge. 2) then
         print*,'x and y'
         call prden8(x,nscr,itrno,6,'(20f8.2)')
         do i=1,irand
            call prden8(y(1,1,i),nscr,itrno,6,'(20f8.2)')
         enddo
      endif

c (semi) diagonalize all y() simulateneously using the FG algorithm by Flury
c  Constantine
c                  (testing bypass for one random effect)
c      if (irand .eq. 1) then
c           call jacobi(y,itrno,nscr,d1,z,nrot)
c           call posdef(y,itrno,nscr,d1,z,flag)
c           if (flag .gt. 0) then
c              print*,'y(',i,') not positive definite'
c           endif
c           do i=1,itrno
c              y(i,i,1)=d1(i)
c           enddo
c        else
         do i=1,irand
            call jacobi(y(1,1,i),itrno,nscr,d1,z,nrot)
            call posdef(y(1,1,i),itrno,nscr,d1,z,flag)
            if (flag .gt. 0) then
              print*,'y(',i,') not positive definite'
            endif
          enddo

          call fgalgm(y,weight,z,itrno,irand,flag,nsweep)
          if (flag .ne. 0) then
             print*,'flag=',flag,' in multiple diagonalization'
             if (flag .le. 3) stop
          endif
c      endif

      if (msglev .ge. 2) then
         print*,'z and y after fg'
         call prden8(z,nscr,itrno,6,'(20f8.2)')
         do i=1,irand
            call prden8(y(1,1,i),nscr,itrno,6,'(20f8.2)')
         enddo
      endif

c print weigthed off-diagonals for measure of diagonalization
      if (nscr .gt. irand) then
         do i=1,irand
            scr(i)=0
            do j=1,itrno
               do k=j+1,itrno
                  scr(i)=scr(i)+abs(y(j,k,i)**2/y(j,j,i)/y(k,k,i))
               enddo
            enddo
            scr(i)=sqrt(+1e-20+scr(i))/(itrno*(itrno-1)/2)
         enddo
         print 10,(scr(i),i=1,irand)
         write(7,10)(scr(i),i=1,irand)

10       format(' relative off-diagonals in G after diagonalization:',
     +            5g10.2)
      endif


c calculate the transformation W=Z'D(-.5)X'
c also winv = inv(W) = X D(-.5) Z
      do i=1,itrno
         do j=1,itrno
            do k=1,itrno
               winv(i,k)=winv(i,k)+x(i,j)*sqrt(d(j))*z(j,k)
               w(i,k)=w(i,k)+z(j,i)/sqrt(d(j))*x(k,j)
            enddo
         enddo
      enddo

      do  i=1,itrno
         do j=1,irand
           varw(i,j)=1/y(i,i,j)
         enddo
      enddo

      if (msglev .ge. 2) then
         print*,'w'
         call prden8(w,ntrait,itrno,6,'(20f8.4)')
      endif

      end



      subroutine prden(x,n,m,un,form)
c prints real matrix x(n,n), first m rows and columns
      integer n,m,i,j,un
      character*(*) form
      real x(n,n)
      do 10 i=1,m
10       write(un,form)(x(i,j),j=1,m)
      write(un,*)
      end

      subroutine prden8(x,n,m,un,form)
c prints real matrix x(n,n), first m rows and columns
      integer n,m,i,j,un
      character*(*) form
      real*8 x(n,n)
      do 10 i=1,m
10       write(un,form)(x(i,j),j=1,m)
      write(un,*)
      end

            SUBROUTINE JACOBI(A,N,NP,D,V,NROT)
      integer nmax,n,np,nrot,ip,iq,i,j
      PARAMETER (NMAX=100)
      real*8 A(NP,NP),D(NP),V(NP,NP),B(NMAX),Z(NMAX)
      real*8 sm,tresh,g,h,t,theta,c,s,tau
      DO 12 IP=1,N
        DO 11 IQ=1,N
          V(IP,IQ)=0.
11      CONTINUE
        V(IP,IP)=1.
12    CONTINUE
      DO 13 IP=1,N
        B(IP)=A(IP,IP)
        D(IP)=B(IP)
        Z(IP)=0.
13    CONTINUE
      NROT=0
      DO 24 I=1,50
        SM=0.
        DO 15 IP=1,N-1
          DO 14 IQ=IP+1,N
            SM=SM+ABS(A(IP,IQ))
14        CONTINUE
15      CONTINUE
        IF(SM.EQ.0.)RETURN
        IF(I.LT.4)THEN
          TRESH=0.2*SM/N**2
        ELSE
          TRESH=0.
        ENDIF
        DO 22 IP=1,N-1
          DO 21 IQ=IP+1,N
            G=100.*ABS(A(IP,IQ))
            IF((I.GT.4).AND.(ABS(D(IP))+G.EQ.ABS(D(IP)))
     *         .AND.(ABS(D(IQ))+G.EQ.ABS(D(IQ))))THEN
              A(IP,IQ)=0.
            ELSE IF(ABS(A(IP,IQ)).GT.TRESH)THEN
              H=D(IQ)-D(IP)
              IF(ABS(H)+G.EQ.ABS(H))THEN
                T=A(IP,IQ)/H
              ELSE
                THETA=0.5*H/A(IP,IQ)
                T=1./(ABS(THETA)+SQRT(1.+THETA**2))
                IF(THETA.LT.0.)T=-T
              ENDIF
              C=1./SQRT(1+T**2)
              S=T*C
              TAU=S/(1.+C)
              H=T*A(IP,IQ)
              Z(IP)=Z(IP)-H
              Z(IQ)=Z(IQ)+H
              D(IP)=D(IP)-H
              D(IQ)=D(IQ)+H
              A(IP,IQ)=0.
              DO 16 J=1,IP-1
                G=A(J,IP)
                H=A(J,IQ)
                A(J,IP)=G-S*(H+G*TAU)
                A(J,IQ)=H+S*(G-H*TAU)
16            CONTINUE
              DO 17 J=IP+1,IQ-1
                G=A(IP,J)
                H=A(J,IQ)
                A(IP,J)=G-S*(H+G*TAU)
                A(J,IQ)=H+S*(G-H*TAU)
17            CONTINUE
              DO 18 J=IQ+1,N
                G=A(IP,J)
                H=A(IQ,J)
                A(IP,J)=G-S*(H+G*TAU)
                A(IQ,J)=H+S*(G-H*TAU)
18            CONTINUE
              DO 19 J=1,N
                G=V(J,IP)
                H=V(J,IQ)
                V(J,IP)=G-S*(H+G*TAU)
                V(J,IQ)=H+S*(G-H*TAU)
19            CONTINUE
              NROT=NROT+1
            ENDIF
21        CONTINUE
22      CONTINUE
        DO 23 IP=1,N
          B(IP)=B(IP)+Z(IP)
          D(IP)=B(IP)
          Z(IP)=0.
23      CONTINUE
24    CONTINUE
      PAUSE '50 iterations should never happen'
      RETURN
      END






c  this file includes asr 71 and asr 74 which provide replacements for
c  subroutines falg and galg respectively.   the replacements have been
c  placed at the end.
c
      subroutine fgalgm(a, rn, b, ip, k, ifault, nf)
      integer jend,l,ip1,j,i,k,ip,nf,ifault
c
c        algorithm as211  appl. statist. (1985) vol. 34, no. 2
c        f-g diagonalization algorithm
c
c modified by I. Misztal for different index order
c
      real*8 a(20, 20, 5), h(5, 20, 20), rn(5), b(20, 20)
      real*8 zero, one, p8, p6, g1, g2
      data zero /0.0/, one /1.0/, p8 /0.8/, p6 /0.6/
c
c        check input parameters
c
      ifault = 1
      if ((k .lt. 1).or.(k .gt. 5)) return
      ifault = 2
      if ((ip .lt. 2).or.(ip .gt. 20)) return
      ifault = 3
      do 3 i = 1, k
      if (rn(i)  .le.  zero) return
    3 continue
      do 4 i = 1, k
      ifault = -i
      do 4 l = 2, ip
      jend = l - 1
      do 4 j = 1, jend
      if (a(l, j, i)  .ne.  a(j, l, i)) return
    4 continue
      ifault = 0
c
      do 5 l = 1, ip
      do 5 j = 1, ip
      b(l, j) = zero
      if (l .eq. j) b(l, j) = one
    5 continue
c
      do 7 i = 1, k
      do 7 l = 1, ip
      do 7 j = 1, ip
    7 h(i, l, j) = a(l, j, i)
      call falg(a, rn, b, ip, k, ifault, nf)
      if ((ifault .ne. 0).or.(nf .gt. 1)) return
c
c        if f-algorithm stopped after only 1 iteration, try
c        another initial approximation to the orthogonal matrix b.
c
      do 8 i = 1, k
      do 8 l = 1, ip
      do 8 j = 1, ip
    8 a(l, j, i) = h(i, l, j)
      ip1 = ip - 1
      do 6 l = 1, ip1
      do 6 j = 1, ip
      g1 = p8 * b(l, j) + p6 * b(l + 1, j)
      g2 = p8 * b(l + 1, j) - p6 * b(l, j)
      b(l, j) = g1
      b(l + 1, j) = g2
    6 continue
      call falg(a, rn, b, ip, k, ifault, nf)
      return
      end
c
c
c
      subroutine mult(a, b, ip, iq)
      integer j,k,i,ip,iq
      real*8 a(20, 20), b(20, 20), h(20, 20)
      real*8 zero
      data zero /0.0/
c
      do 1 i = 1, iq
      do 1 j = 1, ip
      h(i, j) = zero
      do 1 k = 1, ip
      h(i, j) = h(i, j) + b(k, i) * a(k, j)
    1 continue
      do 2 i = 1, iq
      do 2 j = 1, i
      a(i, j) = zero
      do 3 k = 1, ip
    3 a(i, j) = a(i, j) + h(i, k) * b(k, j)
      a(j, i) = a(i, j)
    2 continue
      return
      end
c
c---------------------------------------------------------------------
c
      subroutine falg(a, rn, b, ip, k, ifault, nf)
c
c        asr 71 (remark on as 211) appl. statist. (1988) vol. 37, no. 1
c
      integer i, ifault, ip, ip1, j, jstart, k, l, m, nf, maxf
      real*8 a(20, 20, 5), aux(20, 20), b(20, 20), b1, b2, bold(20, 20)
      real*8 c, c2, diff, eps, epsf, q(2, 2), r1, rn(5), s, t(5, 2, 2)
      real*8 t1, t2, zero, one, two
c STORAGE FOR REORTHOGONALIZATION
      real*8 SCR1(20,20),SCR2(20,20)
      data zero /0.0/, one /1.0/, two /2.0/, epsf /1d-10/, maxf /15/
c
      nf = 0
      ifault = 0
      ip1 = ip - 1
c
c        initial multiplication
c
      do 2 i = 1, k
      do 1 l = 1, ip
      do 1 j = 1, ip
    1 aux(l, j) = a(l, j, i)
      call mult(aux, b, ip, ip)
      do 2 l = 1, ip
      do 2 j = 1, ip
    2 a(l, j, i) = aux(l, j)
c
c        start iteration step of f-algorithm
c
    4 nf = nf + 1
      do 5 l = 1, ip
      do 5 j = 1, ip
    5 bold(l, j) = b(l, j)
      do 8 l = 1, ip1
      jstart = l + 1
      do 8 j = jstart, ip
      do 6 i = 1, k
      t(i, 1, 1) = a(l, l, i)
      t(i, 1, 2) = a(l, j, i)
      t(i, 2, 1) = a(j, l, i)
      t(i, 2, 2) = a(j, j, i)
    6 continue
c
c        get rotation sine and cosine
c
      call galg(t, q, rn, k, ifault)
      if (ifault  .ne.  0) return
c
c        rotate b
c
      c = q(1, 1)
      s = q(2, 1)
      r1 = s / (one + c)
      do 7 m = 1, ip
      b1 = b(m, l)
      b2 = b(m, j)
      b(m, l) = b1 + s * (b2 - r1 * b1)
      b(m, j) = b2 - s * (b1 + r1 * b2)
    7 continue
c
c        update the a matrices
c
      t1 = s / c
      t2 = t1 * t1
      c2 = c * c
      do 8 i = 1, k
      a(l, l, i) = c2 * (t(i, 1, 1) + two * t1 * t(i, 1, 2) + t2 *
     *  t(i, 2, 2))
      a(j, j, i) = c2 * (t2 * t(i, 1, 1) - two * t1 * t(i, 1, 2)
     *  + t(i, 2, 2))
      a(l, j, i) = c2 * (t1 * (t(i, 2, 2) - t(i, 1, 1)) + (one - t2)
     *  * t(i, 1, 2))
      a(j, l, i) = a(l, j, i)
      do 8 m = 1, ip
      if ((m .eq. j).or.(m .eq. l)) goto 8
      b1 = a(m, l, i)
      b2 = a(m, j, i)
      a(m, l, i) = b1 + s * (b2 - r1 * b1)
      a(m, j, i) = b2 - s * (b1 + r1 * b2)
      a(l, m, i) = a(m, l, i)
      a(j, m, i) = a(m, j, i)
    8 continue
c
c        check for convergence
c
C REORTHOGONALIZE, AS SUGGESTED IN THE BOOK FOR LARGE N
      call qrfact(b,scr1,scr2,ip,20)
      do i=1,ip
         do j=1,ip
            b(i,j)=scr1(i,j)
         enddo
      enddo

      eps = zero
      do 9 l = 1, ip
      do 9 j = 1, ip
      diff = abs(b(l, j) - bold(l, j))
      if (diff .gt. eps) eps = diff
    9 continue
      if ((eps  .ge.  epsf).and.(nf .lt. maxf)) goto 4
      if (eps  .ge.  epsf) ifault = 4
      return
      end
c
c---------------------------------------------------------------------
c
c  this is asr 74
c
      subroutine galg (t, q, rn, k, ifault)
      integer i,ifault,k
      real*8 c, co, eight, epsg, fmg, four, h, pi, q(2,2), rn(5),
     &     si,  t(5,2,2), theta, two, u, wt, zero
c
      data epsg/1.0d-14/, zero/0.0d0/, two/2.0d0/, four/4.0d0/,
     &     eight/8.0/, pi/3.141592653589793238/
c
      fmg = zero
      h = zero
c                                  compute iteration constants
      do 1  i=1, k
      u  = (t(i,2,2)-t(i,1,1))/two
      c  = t(i,1,2)
      wt = rn(i)
      fmg = fmg + wt*(u*u-c*c)
    1 h = h + wt*u*c
c                                  check for degenerate cases
      if ((abs(h) .lt. epsg).or.(abs(fmg) .lt. epsg)) goto 2
c
c                                  nodegenerate cases (no. 1 to 4)
c
      theta = atan(-two*h/fmg)/four
c                                 cases 2 and 4
      if ((h.gt.zero).and.(fmg.lt.zero)) theta = theta - pi/four
      if ((h.lt.zero).and.(fmg.lt.zero)) theta = theta + pi/four
      go to 3
c
c                                  degenerate cases (no. 5 to 9)
c
    2 theta = zero
      if (fmg  .le.  -epsg) theta = -pi/four
      if (h  .ge.  epsg) theta = -pi/eight
      if (h  .le.  -epsg) theta = pi/eight
c                                  get cosines and sines from angle
    3 si     = sin(theta)
      co     = cos(theta)
      q(1,1) = co
      q(1,2) = -si
      q(2,1) = si
      q(2,2) = co
      return
      end

      subroutine qrfact(a,q,r,n,np)
c Modified Gram-Schmidt qr factorization: a=qr for real matrices based
c on algorithm 5.2.5 in "matrix computations" by Golub and Van Loan.
c  Matrices a, q and r are declared as np x np and have size  n
      integer n,np,j,k,l
      real*8 a(np,np),q(np,np),r(np,np)

c for k=1:n
c
      do k=1,n
c
c     r(k,k)=||a(1:m,k)/r(k,k)
c
        r(k,k)=0
        do l=1,n
           r(k,k)=r(k,k)+a(l,k)**2
        enddo
        r(k,k)=sqrt(r(k,k))
c
c     q(1:m,k)=a(1:m,k)/r(k,k)
c
        do l=1,n
           q(l,k)=a(l,k)/r(k,k)
        enddo
c
c     for j=k+1:n
c
        do j=k+1,n
c
c        r(k,j)=q(1:m,k)'a(1:m,j)
c
           r(k,j)=0
           do l=1,n
              r(k,j)=r(k,j)+q(l,k)*a(l,j)
           enddo
c
c        a(1:m,j)=a(1:m,j)-q(1:m,k)r(k,j)
c
           do l=1,n
              a(l,j)=a(l,j)-q(l,k)*r(k,j)
           enddo
         enddo
      enddo
      end



      subroutine posdef(g,itrno,ntrait,d,x,ineg)
c checks for positive-definitness of G. If not, zeroes
c negative eigevalues and restores G.
      integer itrno,ntrait,ineg,i,j,k
      real*8 g(ntrait,ntrait),x(ntrait,ntrait),d(itrno)
c restore upper-diagonal elements of G destroyed by Jacobi
      call restor(g,itrno,ntrait)
      ineg=0
      do 10 i=1,itrno
         if(d(i).lt.0) then
             ineg=ineg+1
             do 20 j=1,itrno
                do 20 k=1,itrno
20                 g(j,k)=g(j,k)-d(i)*x(i,k)*x(i,j)
             d(i)=1.0e-10
         endif
10    continue
      end


      subroutine restor(g,itrno,ntrait)
c restores symmetric g which upper diagonal elements have been destroyed
      integer ntrait,itrno,i,j
      real*8 g(ntrait,ntrait)
      do 10 i=1,itrno-1
         do 10 j=i+1,itrno
10          g(i,j)=g(j,i)
      end



      integer function hashan(in,kma,table,mxhash)
      integer mxhash,in,jump/16/,try,kma
      integer table(0:mxhash)
      hashan=mod(in,mxhash)
      try = 0
      do while(table(hashan) .ne. in)
        try = try + 1
        if (table(hashan) .eq. 0) then
          table(hashan) = in
          kma = kma + 1
          return
        endif
        if (try .gt. .2*mxhash) then
         print *,'Can not find place after ',try,' tries'
         stop 32
        endif
        hashan = mod(hashan+jump,mxhash)
      enddo
      return
      end


      SUBROUTINE INVRT(A,IA,N)
      integer j,k,n,i,ia
      REAL*4 A(IA,IA),ZERO,XX,SAVE,AIK
      IF(N.GT.IA) GO TO 201
      ZERO=1.D-12
      DO 80 I=1,N
         XX=ABS(A(I,I))
C                              CHECK FOR SINGULARITY
         IF(XX.LT.ZERO) GO TO 203
C                              TRANSFORM THE MATRIX
         SAVE=1.D0/A(I,I)
         DO 50 J=1,N
50         A(J,I)=A(J,I)*SAVE
         A(I,I)=SAVE
         DO 70 K=1,N
           IF(A(I,K) .EQ. 0.D0) GO TO 70
           IF(K.EQ.I)GO TO 70
           AIK=A(I,K)
           DO 60 J=1,N
60           A(J,K)=A(J,K)-A(J,I)*AIK
           A(I,K)=-1.D0*SAVE*AIK
70         CONTINUE
80      CONTINUE
      RETURN
201   PRINT 202
202   FORMAT('0MATRIX DIMENSION LARGER THAN DECLARED')
      STOP
203   PRINT 204
204   FORMAT('0DIAGONAL ELEMENT = 0 ENCOUNTERED IN SUBR INVRT')
      STOP
      END

