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
real :: var_e,var_a
real,allocatable :: a(:,:)
character (40) :: parfile,datafile,pedfile,random

neq=sum(nlev)
print*,'# equations =',neq
xx=0; xy=0; sol=0
!
open(1,file=datafile)
!
do
if (io.ne.0) exit
do i=1,neff
do j=1,neff
enddo
enddo
enddo

do i=1,neff
call ainv(i)
call printnice(a,'(20f10.4)')
elseif(random == 'diag') then
call diag(i)
else
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

integer :: i
do i=1,neff
enddo
end subroutine

! returns address for given level l of effect e and trait t
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
if(io /= 0) exit
nped=nped+1
enddo
print*,'# records in pedigree =',nped
allocate(a(nped,nped))
a=0
rewind 2
do
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
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)
xx(m,m)=xx(m,m)+1.0/var_a
enddo

end subroutine

integer :: i

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

print*,'Data file: ',datafile

print*,'Pedigree file: ',pedfile

open(2,file=pedfile)

print*,'# effects =',neff

allocate(nlev(neff))

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

print*,'Random effect = ',random

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