! Fortran program to open, clost, read and write data from a file
! This is basic file input/output in fortran

program outputdata
    implicit none

    real, DIMENSION(100) :: x, y, p, q
    integer :: i

    do i = 1,100
        x(i) = i * 0.1
        y(i) = sin(x(i)) * (1 - cos(x(i)/3.0))
    end do

    ! Output the data to a dat file
    open(1, file = 'data.dat', status = 'new')
    do i = 1,100
        write(1,*) x(i), y(i)
    end do
    close(1)

    ! opening a file for reading

    open(2, file = 'data.dat', status = 'old')
    do i = 1,100
        read(2,*) p(i), q(i)
    end do
    close(2)

    do i = 1,100
        write(*,*) p(i), q(i)
    end do

end program outputdata


!  OndedimensionUnsteadyHeatConduct.f90
!
!  Program:
!  OndedimensionUnsteadyHeatConduct - Compute Onedimension Heat Transfer Problem
!  Author: Yang Yang
!  October. 2013.

!****************************************************************************
!
!  PROGRAM: OndedimensionUnsteadyHeatConduct
!
!  PURPOSE: Compute Onedimension Heat Conduct Problem in the Wall with Convection Boundary Condition
!
!****************************************************************************

PROGRAM OndedimensionUnsteadyHeatConduct
IMPLICIT NONE

! Basic Physics Parameters: data for metal iron
REAL*8:: k=80.2,rho=7870,cp=447,L=10,h=300,Tf=300,Ti=900
! Basic Mesh Parameters:
INTEGER Nx,Nt
PARAMETER(Nx=100,Nt=100)
REAL*8:: dx,dt,x(Nx),t(Nt)
! Temperature Field Variables:
REAL*8,DIMENSION(Nt,Nx)::Temperature
! Process Varibles:
REAL*8:: e(2:Nx),b(1:Nx),c(1:Nx-1),f(1:Nx),Temp(Nx),Fo,Bi
! Control Varibles:
INTEGER:: i,j

! out put file directory
OPEN(2,file='space_time.txt')
OPEN(3,file='result.txt')

! Mesh step length
dx=L/(Nx-1)
dt=2000
x(1) = 0
DO j=2,Nx
   x(j) = x(j-1) + dx
END DO
t(1) = 0
DO j=2,Nt
   t(j) = t(j-1) + dt
END DO

! Compute Fo and Bi (dimensionless)
Fo = (k/(rho*cp)) *dt / (dx**2)
Bi = h*dx / (k)

! Initial Temperature Field
DO i=1,Nt
   DO j=1,Nx
      Temperature(i,j) = 0
   END DO
END DO

DO j=1,Nx
   Temperature(1,j) = Ti
END DO

! main time loop
! |b1 c1                 |
! |e2 b2 c2              |
! |   e3 b3 c3           |
! |      e4 b4 c4        |
! |          .  .  .     |
! |             .  . cn-1|
! |                en  bn|
DO i=2,Nt
   ! Assemble Sparse matrix A
    DO j = 2,Nx-1
       c(j) = -Fo
       e(j) = -Fo
    END DO
    c(1)   = -2*Fo
    e(Nx)  = -2*Fo

    DO j=2,Nx-1
       b(j) = (1+2*Fo)
    END DO
    b(1)  = 1+2*Bi*Fo+2*Fo
    b(Nx) = 1+2*Bi*Fo+2*Fo

    DO j=2,Nx-1
       f(j)= Temperature(i-1,j)
    END DO
    f(1)  = 2*Bi*Fo*Tf + Temperature(i-1,1)
    f(Nx) = 2*Bi*Fo*Tf + Temperature(i-1,Nx)
    DO
    j=1,Nx
      Temp(j) = Temperature(i-1,j)
    END DO
    ! Inner loop to update temperature field:
    CALL chase(c,e,b,f,Nx,Temp)
    ! Save Result
    DO j=1,Nx
      Temperature(i,j) = Temp(j)
    END DO
    ! Print current time step
    PRINT*, "Current Time Step:",i
END DO

! save result to file
WRITE(2,200)t(:),x(:)
DO i=1,Nt
   WRITE(3,100)Temperature(i,:)
END DO
100 FORMAT(100(f15.8))
200 FORMAT(100(f12.5))

! Body of OndedimensionUnsteadyHeatConduct
PRINT *, 'Computing Job Compelete!'
PAUSE
END PROGRAM OndedimensionUnsteadyHeatConduct


!===============================================================================
!> Compute time derivative from a sequence of time slices, using Lagrange
!> interpolation
!===============================================================================
  MODULE time_slices_mod
  USE io_mod
  USE io_unit_mod
  USE trace_mod
  USE patch_mod
  USE kinds_mod
  USE lagrange_mod
  implicit none
  private
  type, public:: time_slices_t
    integer:: order=3
  contains
    procedure:: init
    procedure, nopass:: interpolate
    procedure, nopass:: derivative1
    procedure, nopass:: derivative2
  end type
  integer, save:: verbose=0
  integer, save:: order=3
  integer, save:: id_debug=0
  type(time_slices_t), public:: time_slices
CONTAINS

!===============================================================================
!===============================================================================
SUBROUTINE init (self, nt)
  class(time_slices_t):: self
  integer:: nt
  !.............................................................................
  integer:: iostat
  namelist /time_slices_params/ verbose, order, id_debug
  logical, save:: first_time=.true.
  !-----------------------------------------------------------------------------
  call trace%begin ('time_slices_t%init')
  !$omp critical (input_cr)
  if (first_time) then
    first_time = .false.
    rewind (io_unit%input)
    read (io_unit%input, time_slices_params, iostat=iostat)
    order = min(order,nt-2)
    write (io%output, time_slices_params)
    write (io_unit%nml, time_slices_params)
    flush (io_unit%nml)
    call lagrange%test
  end if
  !$omp end critical (input_cr)
  self%order = order
  call trace%end()
END SUBROUTINE init

!===============================================================================
!> Time interpolation.  mem() is assumed to contain nt time slices of a 3-D
!> variable, with times consistent with the times produced by patch%timeslots()
!===============================================================================
SUBROUTINE interpolate (patch, mem, buffer)
  class(patch_t):: patch
  integer:: iv
  real(kind=KindScalarVar), dimension(:,:,:,:):: mem
  real(kind=4), dimension(:,:,:):: buffer
  !.............................................................................
  real(8):: times(patch%nt-1)
  integer:: iit(patch%nt-1)
  real(8), dimension(order+1):: w
  integer:: i1, i2
  !-----------------------------------------------------------------------------
  if (verbose > 0) write(io_unit%log,*) patch%id, 'time_slices_t%interpolate'
  call patch%timeslots (iit, times)
  call lagrange%sequence_weights (patch%out_next, times, i1, i2, w, order)
  call weighted_sum (patch, times, w, i1, i2, mem, buffer)
END SUBROUTINE interpolate

!===============================================================================
!> First time derivate
!===============================================================================
SUBROUTINE derivative1 (patch, mem, buffer)
  class(patch_t):: patch
  integer:: iv
  real(kind=KindScalarVar), dimension(:,:,:,:):: mem
  real(kind=4), dimension(:,:,:):: buffer
  !.............................................................................
  real(8):: times(patch%nt-1)
  real(8), dimension(order+1):: w
  integer:: i1, i2
  !-----------------------------------------------------------------------------
  if (verbose > 0) write(io_unit%log,*) patch%id, 'time_slices_t%derivative1'
  times = patch%t(patch%iit(1:patch%nt-1))
  call lagrange%deriv_sequence_weights (patch%out_next, times, i1, i2, w, order)
  call weighted_sum (patch, times, w, i1, i2, mem, buffer)
END SUBROUTINE derivative1

!===============================================================================
!> Second time derivate
!===============================================================================
SUBROUTINE derivative2 (patch, mem, buffer)
  class(patch_t):: patch
  integer:: iv
  real(kind=KindScalarVar), dimension(:,:,:,:):: mem
  real(kind=4), dimension(:,:,:):: buffer
  !.............................................................................
  real(8):: times(patch%nt-1)
  real(8), dimension(order+1):: w
  integer:: i1, i2
  !-----------------------------------------------------------------------------
  if (verbose > 0) write(io_unit%log,*) patch%id, 'time_slices_t%derivative2'
  times = patch%t(patch%iit(1:patch%nt-1))
  call lagrange%deriv2_sequence_weights (patch%out_next, times, i1, i2, w, order)
  call weighted_sum (patch, times, w, i1, i2, mem, buffer)
END SUBROUTINE derivative2

!===============================================================================
!> Weighted sum of time slice values
!===============================================================================
SUBROUTINE weighted_sum (patch, times, w, i1, i2, mem, buffer)
  class(patch_t):: patch
  real(8), dimension(:):: times, w
  integer:: i1, i2
  real(kind=KindScalarVar), dimension(:,:,:,:):: mem
  real(kind=4), dimension(:,:,:):: buffer
  !.............................................................................
  integer:: i
  !-----------------------------------------------------------------------------
  if (verbose>0 .or. patch%id==id_debug) then
    write (io_unit%output,'(a,2i4,2x,10i4)')      'indices:', i1, i2, patch%iit(i1:i2)
    write (io_unit%output,'(a,f12.6,2x,10f14.6)') '  times:', patch%out_next, times(i1:i2)
    write (io_unit%output,'(a,f12.6,2x,10f14.3)') 'weights:', sum(w), w
  end if
  buffer = 0.0
  do i=i1,i2
    buffer = buffer + w(1+i-i1)*mem(:,:,:,i)
  end do
END SUBROUTINE weighted_sum

END MODULE time_slices_mod


!##############################################################################
! MODULE Volume
!
! This code is published under the GNU General Public License v3
!                         (https://www.gnu.org/licenses/gpl-3.0.en.html)
!
! Authors: Hans Fehr and Fabian Kindermann
!          contact@ce-fortran.com
!
! #VC# VERSION: 1.0  (23 January 2018)
!
!##############################################################################
module Volume

    implicit none

    ! module parameter
    real*8, parameter :: pi = 3.14159265358d0

! separates variable declarations from subroutine and functions
contains

    ! for calculating volume of a sphere
    function vol(r)

        implicit none

        ! input and output variables
        real*8, intent(in) :: r
        real*8 :: vol

        ! calculation
        vol = 4d0/3d0*r**3*pi

    end function

end module


! Print out the analytic solution for the homologous dust collapse problem,
! from Colgate and White 1966, ApJ, 143, 626.
!
! This will yield r(t) as a function of t

module constants_module

  implicit none

  ! fundamental constants
  double precision, parameter :: Gconst = 6.67428d-8      ! cm^3/g/s^2
  double precision, parameter :: pi = 3.14159265358979323846d0
  double precision, parameter :: SMALL = 1.d-20

  ! user-defined problem parameters
  double precision, parameter :: rho_0 = 1.d9
  double precision, parameter :: r_0   = 6.5d8

  ! we try to do equal timesteps, but eventually, the radius of the
  ! sphere is changing so quickly that we need to adjust the size of
  ! the timestep.  The code will take nstep timesteps, with the
  ! initial timestep to be (t_f - t_i)/(nstep - 1).  Once the radius
  ! begins to change significantly, we will begin halving the timestep
  ! as needed.

  integer :: nstep = 100                         ! number of points to output
  double precision, parameter :: t_i = 1.d-3     ! initial output time
  double precision, parameter :: t_f = 0.1       ! final output time

end module constants_module

program analytic

  use constants_module

  implicit none

  ! local variables
  double precision :: r, r_guess, r_old, t
  integer :: i, n
  double precision :: dt
  double precision :: dr, f, dfdr

  logical :: converged
  double precision, parameter :: TOL = 1.d-6
  integer, parameter :: max_iter = 25

  ! compute the initial timestep
  dt = (t_f - t_i)/(nstep - 1)

  r_old = r_0

  t = t_i

  ! main timestepping loop
  do i = 1, nstep

     ! find the analytic solution via Newton iteration
     converged = .false.
     r_guess = 0.95*r_old

     do n = 1, max_iter
        dr = -f(r_guess,t)/dfdr(r_guess,t)

        ! we have lots of sqrt(1 - r/r_0), so r(t) has to be less than r_0
        if (r_guess + dr > r_0) then
           r_guess = 0.5d0*(r_guess + r_0)
        else
           r_guess = r_guess + dr
        endif

        if (abs(dr/r_guess) < TOL) then
           converged = .true.
           exit
        endif

     enddo

     if (.not. converged) then
        print *, "ERROR: Newton iterations failed to converge"
        stop
     endif

     ! we converged, set the new radius of the sphere
     r = r_guess

     print *, t, r

     ! toward the end, we need smaller timesteps, if the radius is
     ! changing significantly, halve the timestep
     if (abs(r - r_old)/r_old > 0.2) then
        dt = 0.5*dt
     endif

     ! store the old radius for the timestep check next cycle
     r_old = r

     ! update the desired output time
     t = t + dt

  enddo

end program analytic

function f(r,t) result (func)

  use constants_module

  implicit none

  double precision, intent(in) :: r, t
  double precision :: func

  ! the analytic solution from Colgate and White, Eq. 5 (with everything moved
  ! onto the LHS).  We use SMALL to prevent against zeros in some places.
  func = sqrt(8.d0*pi*Gconst*rho_0/3.d0)*t - &
         sqrt(1.d0 - r/r_0)*sqrt(r/r_0) - asin(sqrt(1.0 - r/r_0 + SMALL))

  return
end function f

function dfdr(r,t) result (dfuncdr)

  use constants_module

  implicit none

  double precision, intent(in) :: r, t
  double precision :: dfuncdr

  ! the derivative (wrt x) of the analytic function f, defined above.
  ! We use SMALL to prevent against zeros in some places.
  dfuncdr = (0.5d0/r_0)*(-sqrt(1.d0 - r/r_0 + SMALL)/sqrt(r/r_0) + &
                      sqrt(r/r_0)/sqrt(1 - r/r_0 + SMALL) + &
                      1.d0/(sqrt(r/r_0)*sqrt(1.d0 - r/r_0 + SMALL)))

  return
end function dfdr


      SUBROUTINE ALG29 (Y,X,FXY,N)
C
      DIMENSION Y(3),X(3),FXY(3)
C
      X1=(X(3)+X(2))*(Y(3)-Y(2))/(X(3)-X(2))
      FXY(2)=X1/(X(3)-X(1))
      N2=N-2
      DO 100 J=3,N2
      X2=(X(J+1)+X(J))*(Y(J+1)-Y(J))/(X(J+1)-X(J))
      FXY(J)=(X2-X1)/(X(J+1)-X(J-1))
100   X1=X2
      FXY(N-1)=-X1/(X(N)-X(N-2))
      FXY(1)=FXY(2)-(FXY(3)-FXY(2))/(X(3)-X(2))*(X(2)-X(1))
      FXY(N)=FXY(N-1)+(FXY(N-1)-FXY(N-2))/(X(N-1)-X(N-2))*(X(N)-X(N-1))
      RETURN
      END


subroutine CalcEnergy(Pos, M, L, rc, PEnergy, Dim, NAtom)
    implicit none
    integer, intent(in) :: M, Dim, NAtom
    real(8), intent(in), dimension(0:NAtom-1, 0:Dim-1) :: Pos
    real(8), intent(in) :: L, rc
    real(8), intent(out) :: PEnergy
    real(8), parameter :: k = 3000., r0 = 1.
    integer:: i, j
    real(8) :: kfac, d2, sep, disp, shiftval, rc2, id6, id2, id12
    real(8), dimension(Dim) :: rij, Posi

    !Assign initially zeros to the force array and potential energy
    PEnergy = 0.
    rc2 = rc**2

    !Precompute constants such as LJ shift value up here
    kfac = k/2.
    id6 = 1./rc**6
    shiftval = 4*(id6**2 - id6)

    do i=0, NAtom - 1
        !store Pos(i,:) in a temporary array for faster access in j looop
        Posi = Pos(i,:)
        do j = i+1, NAtom - 1
            !compute initial rij
            rij = Pos(j,:) - Posi
            !Apply minimum image convention
            rij = rij - L*dnint(rij / L)

            !Do bonded calculation
            if ( j == i+1 .and. mod(j, M)> 0) then
                !Compute scalar separation
                sep = sqrt( sum( rij * rij ) )
                !Compute displacement relative to preferred separation
                disp = sep - r0
                !Calculate energy
                PEnergy = PEnergy + kfac * disp*disp
            else   !Do nonbonded calculation
                !compute scalar distance
                d2 = sum(rij*rij)
                !Check against cutoff
                if ( d2 > rc2) then
                    cycle
                end if
                id2 = 1./d2                 !inverse squared distance
                id6 = id2 * id2 * id2       !inverse sixth distance
                id12 = id6 * id6            !inverse twelfth distance
                PEnergy = Penergy + 4. * (id12 - id6) + shiftval

                !end nonbonded calculation
            endif
        enddo
    enddo
end subroutine

!I AM WRITING THIS PART
!added atomnum argument for atom for which to calculate interaction energy
subroutine CalcInteractionEnergy(Pos, atomnum, M, L, rc, PEnergy, Dim, NAtom)
    implicit none
    integer, intent(in) :: M, Dim, NAtom, atomnum
    real(8), intent(in), dimension(0:NAtom-1, 0:Dim-1) :: Pos
    real(8), intent(in) :: L, rc
    real(8), intent(out) :: PEnergy
    real(8), parameter :: k = 3000., r0 = 1.
    integer:: j
    real(8) :: kfac, d2, sep, disp, shiftval, rc2, id6, id2, id12
    real(8), dimension(Dim) :: rij, Posi

    !Assign initially zeros to the force array and potential energy
    PEnergy = 0.
    rc2 = rc**2

    !Precompute constants such as LJ shift value up here
    kfac = k/2.
    id6 = 1./rc**6
    shiftval = 4*(id6**2 - id6)

    !store Pos(i,:) in a temporary array for faster access in j looop
    Posi = Pos(atomnum,:)
    do j = 0, NAtom - 1
        if (j==atomnum) cycle
        !compute initial rij
        rij = Pos(j,:) - Posi
        !Apply minimum image convention
        rij = rij - L*dnint(rij / L)
        !Do bonded calculation
        !Note that each trial movement may involves 2 bond length changes.
        if ( j == atomnum+1 .and. mod(j, M)> 0) then
            !Compute scalar separation
            sep = sqrt( sum( rij * rij ) )
            !Compute displacement relative to preferred separation
            disp = sep - r0
            !Calculate energy
            PEnergy = PEnergy + kfac * disp*disp
        else if (j == atomnum-1 .and. mod(j,M) < M-1) then
            !Compute scalar separation
            sep = sqrt( sum( rij * rij ) )
            !Compute displacement relative to preferred separation
            disp = sep - r0
            !Calculate energy
            PEnergy = PEnergy + kfac * disp*disp
        else   !Do nonbonded calculation
            !compute scalar distance
            d2 = sum(rij*rij)
            !Check against cutoff
            if ( d2 > rc2)  cycle
            id2 = 1./d2                 !inverse squared distance
            id6 = id2 * id2 * id2       !inverse sixth distance
            id12 = id6 * id6            !inverse twelfth distance
            PEnergy = Penergy + 4. * (id12 - id6) + shiftval

            !end nonbonded calculation
        endif
    enddo
end subroutine

! a mutant version of CalcInteractionEnergy
! the atom "atomnum" has an extra virtual displacement parameter.
subroutine CalcVirtualEnergy(Pos, atomnum, virtualDisp, M, L, rc, PEnergy, Dim, NAtom)
    implicit none
    integer, intent(in) :: M, Dim, NAtom, atomnum
    real(8), intent(in), dimension(0:NAtom-1, 0:Dim-1) :: Pos
    real(8), intent(in), dimension(0:Dim-1) :: virtualDisp
    real(8), intent(in) :: L, rc
    real(8), intent(out) :: PEnergy
    real(8), parameter :: k = 3000., r0 = 1.
    integer:: j
    real(8) :: kfac, d2, sep, disp, shiftval, rc2, id6, id2, id12
    real(8), dimension(Dim) :: rij, Posi

    !Assign initially zeros to the force array and potential energy
    PEnergy = 0.
    rc2 = rc**2

    !Precompute constants such as LJ shift value up here
    kfac = k/2.
    id6 = 1./rc**6
    shiftval = 4*(id6**2 - id6)

    !store Pos(i,:) in a temporary array for faster access in j looop
    Posi = Pos(atomnum,:) + virtualDisp
    do j = 0, NAtom - 1
        if (j==atomnum) cycle
        !compute initial rij
        rij = Pos(j,:) - Posi
        !Apply minimum image convention
        rij = rij - L*dnint(rij / L)
        !Do bonded calculation
        if ( j == atomnum+1 .and. mod(j, M)> 0) then
            !Compute scalar separation
            sep = sqrt( sum( rij * rij ) )
            !Compute displacement relative to preferred separation
            disp = sep - r0
            !Calculate energy
            PEnergy = PEnergy + kfac * disp*disp
        else if (j == atomnum-1 .and. mod(j,M) < M-1) then
            !Compute scalar separation
            sep = sqrt( sum( rij * rij ) )
            !Compute displacement relative to preferred separation
            disp = sep - r0
            !Calculate energy
            PEnergy = PEnergy + kfac * disp*disp
        else   !Do nonbonded calculation
            !compute scalar distance
            d2 = sum(rij*rij)
            !Check against cutoff
            if ( d2 > rc2)  cycle
            id2 = 1./d2                 !inverse squared distance
            id6 = id2 * id2 * id2       !inverse sixth distance
            id12 = id6 * id6            !inverse twelfth distance
            PEnergy = Penergy + 4. * (id12 - id6) + shiftval

            !end nonbonded calculation
        endif
    enddo
end subroutine

subroutine CalcEnergyForces(Pos, M, L, rc, PEnergy, Forces, Dim, NAtom)
    implicit none
    integer, intent(in) :: M, Dim, NAtom
    real(8), intent(in), dimension(0:NAtom-1, 0:Dim-1) :: Pos
    real(8), intent(in) :: L, rc
    real(8), intent(out) :: PEnergy
    real(8), intent(inout), dimension(0:NAtom-1, 0:Dim-1) :: Forces
!f2py intent(in, out, inplace) :: Forces
    real(8), parameter :: k = 3000., r0 = 1.
    integer :: i, j
    real(8) :: kfac, d2, sep, disp, shiftval, rc2, id6, id2, id12
    real(8), dimension(Dim) :: rij, Fij, Posi

    !Assign initially zeros to the force array and potential energy
    Forces = 0.
    PEnergy = 0.
    rc2 = rc**2

    !Precompute constants such as LJ shift value up here
    kfac = k/2.
    id6 = 1./rc**6
    shiftval = 4.*(id6**2 - id6)

    do i=0, NAtom - 1
        !store Pos(i,:) in a temporary array for faster access in j looop
        Posi = Pos(i,:)
        do j = i+1, NAtom - 1
            !compute initial rij
            rij = Pos(j,:) - Posi
            !Apply minimum image convention
            rij = rij - L*dnint(rij / L)

            !Do bonded calculation
            if ( j == i+1 .and. mod(j, M)> 0) then
                !Compute scalar separation
                sep = sqrt( sum( rij * rij ) )
                !Compute displacement relative to preferred separation
                disp = sep - r0
                !Calculate energy
                PEnergy = PEnergy + kfac * disp*disp
                !Calculate force: - k*displacement in direction of rij
                Fij = k * ( disp ) * rij/sep
                Forces(i,:) = Forces(i,:) + Fij
                Forces(j,:) = Forces(j,:) - Fij
            else   !Do nonbonded calculation
                !compute scalar distance
                d2 = sum(rij*rij)
                !Check against cutoff
                if ( d2 > rc2) then
                    cycle
                end if
                id2 = 1./d2                 !inverse squared distance
                id6 = id2 * id2 * id2       !inverse sixth distance
                id12 = id6 * id6            !inverse twelfth distance
                PEnergy = Penergy + 4. * (id12 - id6) + shiftval
                Fij = rij * (-48. * id12 + 24. * id6) * id2
                Forces(i,:) = Forces(i,:) + Fij
                Forces(j,:) = Forces(j,:) - Fij

                !end nonbonded calculation
            endif
        enddo
    enddo
end subroutine


      SUBROUTINE SOLVSMBE(SMV1,SMV2,SMA11,SMA22,SMA1,SMA2,SMB11,SMB22)
      REAL::SMA12,SMA2,SMA21,SMA1,SMDET,SMA11,SMA22,SMB11,SMB22
      REAL::SMV1,SMV2
C SOLVE 2X2 MATRIX
C CHANGE RECORD
C
      SMA12 = -SMA2
      SMA21 = -SMA1
      SMDET = SMA11*SMA22 - SMA12*SMA21
      IF(SMDET.EQ.0.0)THEN
        PRINT*, 'SINGULAR MATRIX: A11, A12, A21, A22, B11, B22'
        PRINT*, SMA11,SMA12,SMA21,SMA22,SMB11,SMB22
        STOP
      ENDIF
      SMDET = 1.0 / SMDET
      SMV1 = (SMB11*SMA22 - SMB22*SMA12) * SMDET
      SMV2 = (SMB22*SMA11 - SMB11*SMA21) * SMDET
      RETURN
      END


!C++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
!C      Projet MICO - Newton Raphson (ver. FORTRAN 21.03.95)
!C++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

subroutine newtonr(x, y, P)

    implicit none

    integer :: i, nite, compt
    logical :: converged
    double precision :: h, a, b, Vh, Vv, E, Lh, Lv
    double precision :: TOL, u, v
    double precision :: lambda
    double precision, intent(inout) :: P(101), x(101), y(101)
    double precision :: R(2), uu(2)
    double precision :: norm, F(2), Kt(2, 2), Du(2), MAT(2)
    double precision :: invKt(2, 2), dtmKt

    h = 40.
    a = 24.
    b = 40.
    Vh = 160.
    Vv = 200.
    E = 70000.
    Lh = 2*a
    Lv = sqrt(h**2 + (b + a)**2)
    TOL = 1e-5
    i = 0
    lambda = 0.
    uu(1) = 0.
    uu(2) = 0.
    R(1) = 9000.  ! maximum force
    R(2) = 0.

    !C ----converged principale----

    do compt = 0, 100
        lambda = compt/100.

        ! Newton-Raphson
        converged = .false.
        nite = 0
        do while (.not. converged)
            u = uu(1)
            v = uu(2)
            nite = nite + 1
            if (nite > 200) then
                converged = .true.
                print *, "WARNING: max nb of iterations reached!"
            end if

            F(1) = (E*Vv/Lv**4)*(v*v + u*u - 2*h*v + 2*u*(a + b))*(v - h)
            F(2) = (Vv/Lv**4)*(v**2 + u**2 - 2*h*v + 2*u*(a + b))*(u + a + b) &
                   + (8*Vh/Lh**4)*(u**2 + 2*a*u)*(u + a)

            if (lambda /= 0) then
                norm = dsqrt((lambda*R(1) - F(1))**2 + (lambda*R(2) - F(2))**2) &
                       /abs(lambda)/dsqrt(R(1)**2 + R(2)**2)
                ! write(*,*)norm
            else
                norm = 0
            end if

            if (norm > TOL) then
                Kt(1, 1) = E*Vv/(Lv**4)*(2*u + 2*(a + b))*(v - h)
                Kt(1, 2) = E*Vv/(Lv**4)*((v**2 + u**2 - 2*v*h + 2*u*(a + b)) &
                                         + (v - h)*(2*v - 2*h))
                Kt(2, 1) = Vv/(Lv**4)*((v**2 + u**2 - 2*h*v + 2*u*(a + b)) &
                                       + (u + a + b)*(2*u + 2*(a + b))) &
                           + 8*Vh/(Lh**4)*((2*u + 2*a)*(u + a) + (u**2 + 2*a*u))
                Kt(2, 2) = Vv/(Lv**4)*(u + a + b)*(2*v - 2*h)

                MAT(1) = lambda*R(1) - F(1)
                MAT(2) = lambda*R(2) - F(2)

                dtmKt = Kt(1, 1)*Kt(2, 2) - Kt(1, 2)*Kt(2, 1)

                invKt(1, 1) = Kt(2, 2)/dtmKt
                invKt(2, 2) = Kt(1, 1)/dtmKt
                invKt(1, 2) = -1*Kt(1, 2)/dtmKt
                invKt(2, 1) = -1*Kt(2, 1)/dtmKt

                Du(1) = invKt(1, 1)*MAT(1) + invKt(1, 2)*MAT(2)
                Du(2) = invKt(2, 1)*MAT(1) + invKt(2, 2)*MAT(2)

                uu(1) = uu(1) + Du(1)
                uu(2) = uu(2) + Du(2)
            else
                converged = .true.
            end if
        end do

        i = i + 1
        x(i) = uu(2)
        y(i) = uu(1)
        P(i) = lambda*R(1)
    end do

end subroutine newtonr