!###########################################################
! page.81-82 表4.4(例題5)
! n次元ニュートン・ラフソン法のプログラム
! (4元連立非線形方程式)
!
!###########################################################
PROGRAM prog17
IMPLICIT REAL(8)(a-h,o-z)
IMPLICIT INTEGER(i-n)

INTEGER,PARAMETER :: KMAX=100
INTEGER,PARAMETER :: N=4
REAL(8),DIMENSION(N) :: xx,bb
REAL(8),DIMENSION(N,N) :: aa

!---------------
x=-1.d0
y=-2.d0
u=-1.d0
v=1.d0

aa=0.d0
bb=0.d0
delta=1.d-8
!---------------

xx(1)=x
xx(2)=y
xx(3)=u
xx(4)=v
k=0

WRITE(*,"('k=',i2,', x,y,u,v=',1P4E16.8)") k,xx
CALL right(N,xx,bb)

DO k=1,KMAX

bb=-bb
CALL jacobi(N,xx,aa)
CALL gauss(N,aa,bb)

xx=xx+bb

CALL right(N,xx,bb)
fsum=SUM(bb**2)
WRITE(*,*) fsum
WRITE(*,"('k=',i2,', x,y,u,v=',1P4E16.8)") k,xx
IF(fsum<delta**2) GOTO 100

END DO

WRITE(*,*) '解が求まりませんでした'
STOP

100 CONTINUE
WRITE(*,*)
WRITE(*,*) "ニュートン・ラフソン法で求めた解は"
WRITE(*,"('x=',1PE15.8)") xx(1)
WRITE(*,"('y=',1PE15.8)") xx(2)
WRITE(*,"('u=',1PE15.8)") xx(3)
WRITE(*,"('v=',1PE15.8)") xx(4)

WRITE(*,*)
WRITE(*,*)
WRITE(*,*)



WRITE(*,*) '正解：'
DO i=1,3

IF(i==1)THEN
a=-2.63783d0
b=0.d0
ELSEIF(i==2)THEN
a=-1.68108d0
b=3.05043d0
ELSE
a=-1.68108d0
b=-3.05043d0
END IF


ab2=a**2+b**2
f1=a+(a+b)/ab2
f2=b+(a-b)/ab2
f3=a-(a+b)/ab2
f4=b-(a-b)/ab2
WRITE(*,"('s=',SP,1P2E13.5,'*i')") a,b
WRITE(*,"('x=',1PE15.8,' y=',1PE15.8,' u=',1PE15.8,' v=',1PE15.8)") f1/2,f2/2,f3/2,f4/2
END DO

STOP
END PROGRAM prog17
!###########################################################

SUBROUTINE right(N,xx,bb)
IMPLICIT REAL(8)(a-h,o-z)
IMPLICIT INTEGER(i-n)

INTEGER,INTENT(IN) :: N
REAL(8),DIMENSION(N),INTENT(IN) :: xx
REAL(8),DIMENSION(N),INTENT(OUT) :: bb 
bb=0.d0

xu=xx(1)+xx(3)
yv=xx(2)+xx(4)

!bb(1)=xu**3+6.d0*xu**2-21.d0*xu-3.d0*(xu+2.d0)*yv**2+32.d0
!bb(2)=3.d0*xu**2*yv+12.d0*xu*yv-yv**3
bb(1)=xu**3+6.d0*xu**2+21.d0*xu-3.d0*(xu+2.d0)*yv**2+32.d0
bb(2)=3.d0*(xu**2+4.d0*xu+7.d0)*yv-yv**3

bb(3)=(xx(1)**2-xx(2)**2)-(xx(3)**2-xx(4)**2)-1.d0
bb(4)=2.d0*(xx(1)*xx(2)-xx(3)*xx(4))-1.d0

RETURN
END SUBROUTINE right

!###########################################################

SUBROUTINE jacobi(N,xx,aa)
IMPLICIT REAL(8)(a-h,o-z)
IMPLICIT INTEGER(i-n)

INTEGER,INTENT(IN) :: N
REAL(8),DIMENSION(N),INTENT(IN) :: xx
REAL(8),DIMENSION(N,N),INTENT(OUT) :: aa 
aa=0.d0

xu=xx(1)+xx(3)
yv=xx(2)+xx(4)

!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
aa(1,1)=3.d0*xu**2+12.d0*xu+21.d0-3.d0*yv**2
aa(1,2)=-6.d0*(xu+2.d0)*yv
aa(1,3)=aa(1,1)
aa(1,4)=aa(1,2)
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
aa(2,1)=6.d0*xu*yv+12.d0*yv
aa(2,2)=3.d0*xu**2+12.d0*xu+21.d0-3.d0*yv**2
aa(2,3)=aa(2,1)
aa(2,4)=aa(2,2)
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
aa(3,1)= 2.d0*xx(1)
aa(3,2)=-2.d0*xx(2)
aa(3,3)=-2.d0*xx(3)
aa(3,4)= 2.d0*xx(4)
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
aa(4,1)= 2.d0*xx(2)
aa(4,2)= 2.d0*xx(1)
aa(4,3)=-2.d0*xx(4)
aa(4,4)=-2.d0*xx(3)
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!

RETURN
END SUBROUTINE jacobi
!###########################################################

!oooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooo!
SUBROUTINE gauss(n,a,b)
IMPLICIT REAL(8)(a-h,o-z)
IMPLICIT INTEGER(i-n)
INTEGER,INTENT(IN) :: n
REAL(8),INTENT(INOUT),DIMENSION(n,n) :: a
REAL(8),INTENT(INOUT),DIMENSION(n) :: b
REAL(8),DIMENSION(n) :: ip,wk
eps=3.52d-15
ip=0.d0;wk=0.d0

DO k=1,n-1
!  find maximum element in the k-th column.
amax=dabs(a(k,k))
ipk=k

  DO i=k+1,n
  aik=dabs(a(i,k))
    IF(aik>amax) THEN
    ipk=i
    amax=aik
    END IF
  END DO

ip(k)=ipk

!  matrix is singular.
IF(amax<eps) THEN
WRITE(*,*)'(subr.glu) matrix is singular at k =',k
STOP
END IF

      IF(ipk/=k) THEN
      w=a(ipk,k)
      a(ipk,k)=a(k,k)
      a(k,k)=w
      END IF
!  compute alfa
      DO i=k+1,n
      a(i,k)=-a(i,k)/a(k,k)
      wk(i)=a(i,k)
      END DO
      
      DO j=k+1,n
         IF(ipk/=k) THEN
         w=a(ipk,j)
         a(ipk,j)=a(k,j)
         a(k,j)=w
         END IF
!  gaussian elimination
      t=a(k,j)
         DO i=k+1,n
         a(i,j)=a(i,j)+wk(i)*t
         END DO
      END DO
END DO
!  right-hand side
!  forward elimination process
DO k=1,n-1
   IF(ip(k)/=k) THEN
     w=b(ip(k))
     b(ip(k))=b(k)
     b(k)=w
   END IF

   t=b(k)

   DO i=k+1,n
   b(i)=b(i)+a(i,k)*t
   END DO
END DO

!  backward substitution process
DO k=n,2,-1
   b(k)=b(k)/a(k,k)
      t=b(k)
      DO i=1,k-1
      b(i)=b(i)-a(i,k)*t
      END DO
END DO
b(1)=b(1)/a(1,1)

RETURN
END SUBROUTINE gauss
!oooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooo!