program lsq
use printmat2
implicit none
!
!This program calculates least square solutions for a model with 2 effects.
!All the storage is dense, and solutions are obtained iteratively by
!Gauss-Seidel.
! This program is easily upgradable to any number of effects
!
real, allocatable:: xx(:,:),xy(:),sol(:) !storage for the equations
integer, allocatable:: indata(:) !storage for one line of effects
!integer,parameter:: neff=2,nlev(2)=(/2,3/) !number of effects and levels
real :: y ! observation value
integer :: neq,io,i,j ! number of equations and io-status
integer :: neff,addeff
real :: var_e,var_a
integer,allocatable :: nlev(:),address(:)
real,allocatable :: a(:,:)
character (40) :: parfile,datafile,pedfile,random

call read_parameter

neq=sum(nlev)
print*,'# equations =',neq
allocate (xx(neq,neq), xy(neq), sol(neq),indata(neff),address(neff))
xx=0; xy=0; sol=0
!
open(1,file=datafile)
!
do
   read(1,*,iostat=io)indata,y
   if (io.ne.0) exit
   call find_addresses
   do i=1,neff
      do j=1,neff
         xx(address(i),address(j))=xx(address(i),address(j))+1.0/var_e
      enddo
      xy(address(i))=xy(address(i))+y/var_e
   enddo
enddo

do i=1,neff
   if(i == addeff) then
      if(random == 'add') then
         call ainv(i)
         print*,'Additive relationship matrix'
         call printnice(a,'(20f10.4)')
      elseif(random == 'diag') then
         call diag(i)
      else
         print*,'* choose add or diag'
         stop
      endif
   endif
enddo
!
print*,'left hand side'
do i=1,neq
   print '(100f10.4)',xx(i,:)
enddo
!
print '( '' right hand side:'' ,100f10.4)',xy

call solve_dense_gs(neq,xx,xy,sol) !solution by Gauss-Seidel
open(99,file='solutions')
do i=1,neq
   write(99,*) sol(i)
enddo
print '( '' solution:'' ,100f10.4)',sol

contains

subroutine find_addresses
integer :: i
do i=1,neff
   address(i)=sum(nlev(1:i-1))+indata(i)
enddo
end subroutine

function address1(e,l)
! returns address for given level l of effect e and trait t
integer :: e,l,address1
address1= sum(nlev(1:e-1))+(l-1)+1
end function

subroutine ainv(eff)
integer :: eff,i,k,l,m,n,io,p(3),nped=0
real ::w(3),res(4),mendel

w=(/1., -.5, -.5/)
res=(/2., 4/3., 1., 0./)

do
   read(2,*,iostat=io) p
   if(io /= 0) exit
   nped=nped+1
enddo
print*,'# records in pedigree =',nped
allocate(a(nped,nped))
a=0
rewind 2
do
   read(2,*,iostat=io) p
   if (io /= 0) exit
   i=1
   if(p(2) == 0) i=i+1
   if(p(3) == 0) i=i+1
   mendel=res(i)
   do k=1,3
      do l=1,3
         if (p(k) /= 0 .and. p(l) /=0) then
            m=address1(eff,p(k))
            n=address1(eff,p(l))
            a(p(k),p(l))=a(p(k),p(l))+w(k)*w(l)*mendel
            xx(m,n)=xx(m,n)+w(k)*w(l)*mendel/var_a
         endif
      enddo
   enddo
enddo
end subroutine

subroutine diag(eff)
integer :: eff,i,m

do i=1,nlev(eff)
   m=address1(eff,i)
   xx(m,m)=xx(m,m)+1.0/var_a
enddo

end subroutine

subroutine read_parameter
integer :: i

print*,'Parameter file?'
read(*,'(a)') parfile
open(99,file=parfile)

read(99,'(a)') datafile
print*,'Data file: ',datafile

read(99,'(a)') pedfile
print*,'Pedigree file: ',pedfile

open(2,file=pedfile)

read(99,*) neff
print*,'# effects =',neff

allocate(nlev(neff))
read(99,*) nlev

do i=1,neff
   print*,i,'-th effect # levels =',nlev(i)
enddo

read(99,*) addeff
print*,'Additive effect =',addeff

read(99,'(a)') random
print*,'Random effect = ',random

read(99,*) var_a
print*,'Additive variance =',var_a

read(99,*) var_e
print*,'Residual variance =',var_e

end subroutine

end program lsq

subroutine solve_dense_gs(n,lhs,rhs,sol)
! finds sol in the system of linear equations: lhs*sol=rhs
! the solution is iterative by Gauss-Seidel
integer :: n
real :: lhs(n,n),rhs(n),sol(n),eps
integer :: round
!
round=0
do
   eps=0; round=round+1
   do i=1,n
      solnew=sol(i)+(rhs(i)-sum(lhs(i,:)*sol))/lhs(i,i)
      eps=eps+ (sol(i)-solnew)**2
      sol(i)=solnew
   end do
   print*,round,' rounds conv =',eps
   if (eps.lt. 1e-10) exit
end do
print*,'solutions computed in ',round,' rounds of iteration'
end subroutine