PROGRAM MAIN
  IMPLICIT NONE

!  CALL TESTBEADQINT
!  CALL TESTBEADROD
!  CALL TESTMCSAMPLEREL
!  CALL TESTCHAINEQUIL
!  CALL TESTSAMPLEUTILS
!  CALL TESTANALYTIC
!  CALL TESTREADSNAP
!  CALL TESTBROWNDYN
!  CALL TESTCHAINGROUP
  CALL TESTCHAINFORCE
 ! CALL CHECKMINIMONTECARLO
 ! CALL CHECKREMOVEADDBEAD
!  CALL CHECKREDISC
!  CALL  CHECKSPHEREINTERSECT
!  CALL CHECKCUBICROOT
!  CALL TESTCRANK

CONTAINS
  SUBROUTINE TESTBEADQINT
    ! test calculation of bead partition function integrated over R or U
    USE CHAINUTIL
    USE KEYS
    USE SAMPLEUTIL
    IMPLICIT NONE
    TYPE(CHAIN), TARGET :: WLCLIST(1)
    TYPE(CHAIN), POINTER :: CHAINP
    INTEGER :: SC, B, NRHO, NPHI
    DOUBLE PRECISION :: DR(3), LASTCOORDS(6), ENERGY1, ENERGY2, QINT

    CHAINP=>WLCLIST(1); 

    CALL READKEY

    CALL SETUPCHAIN(CHAINP,MAXNPT)
    CALL SETCHAINPARAMS(CHAINP)   
    CALL INITIALIZECHAIN(CHAINP,.TRUE.,INITRANGE)

    print*, 'chain info:', chainp%npt
            
    CALL GETEQUILCHAIN(CHAINP,EQUILSAMPLETYPE,LASTCOORDS)
    B=2
    CALL GETBEADENERGY(CHAINP,B,ENERGY1)
    CALL GETBEADENERGY(CHAINP,B+1,ENERGY2)
    print*, 'current energy:', energy1+energy2

    !CALL GETBEADQUINT(CHAINP,B,NRHO,NPHI,QINT)
    print*, 'testx1:', intrweightnpt
    CALL GETBEADQRINT(CHAINP,B,INTRWEIGHTNPT,QINT)
    PRINT*, 'QINT, free energy:', QINT, -LOG(QINT)

    CALL OUTPUTSNAPSHOT(CHAINP,SNAPSHOTFILE,1,.FALSE.)
 
    CALL CLEANUPCHAIN(CHAINP)
  END SUBROUTINE TESTBEADQINT

  SUBROUTINE TESTBEADROD
    ! test bead-rod forces by comparing to andy's code    
    USE MT19937
    USE CHAINUTIL
    USE KEYS
    USE SAMPLEUTIL
    USE BROWNDYN
    IMPLICIT NONE
    TYPE(CHAIN), TARGET :: WLCLIST(1)
    TYPE(CHAIN), POINTER :: CHAINP
    INTEGER :: b
    DOUBLE PRECISION :: DR(3), LASTCOORDS(6), ENERGY
    DOUBLE PRECISION, ALLOCATABLE :: BROWNFORCE(:,:),RFORCE(:,:)
    DOUBLE PRECISION :: DELT, S2DTR

    CHAINP=>WLCLIST(1); 

    CALL READKEY

    CALL SETUPCHAIN(CHAINP,MAXNPT)
    CALL SETCHAINPARAMS(CHAINP)   
    CALL INITIALIZECHAIN(CHAINP,.TRUE.,INITRANGE)

    CALL GETEQUILCHAIN(CHAINP,2,LASTCOORDS)
    
    ! CALL OUTPUTSNAPSHOT(CHAINP,SNAPSHOTFILE,1,.FALSE.)
    ! OPEN(UNIT=55,FILE='initsnap',status='unknown')
    ! DO B= 1,CHAINP%NPT
    !    print*, b, chainp%pos(:,b)
    !    WRITE(55,*) CHAINP%POS(:,B)
    ! ENDDO
    ! CLOSE(55)

    ALLOCATE(BROWNFORCE(3,CHAINP%NPT),RFORCE(3,CHAINP%NPT))
    DELT = DELTSCL*CHAINP%FRICTR
    !CALL GETBEADRODFORCE(CHAINP,BROWNFORCE,BRCRELAX,RFORCE,ENERGY)    
    !DO B= 1,CHAINP%NPT
    !   print*, b, RFORCE(:,b)
    !ENDDO

    ! ------ test forces---------
    ! S2DTR = SQRT(2*CHAINP%FRICTR/DELT)
    ! OPEN(UNIT=55,FILE='brownpos.out')
    ! DO B = 1,CHAINP%NPT
    !    ! translational brownian force
       
    !    BROWNFORCE(1,B) = RNORM()*S2DTR
    !    BROWNFORCE(2,B) = RNORM()*S2DTR
    !    BROWNFORCE(3,B) = RNORM()*S2DTR
    !    WRITE(55,*) BROWNFORCE(:,B)
    ! ENDDO
    ! CLOSE(55)
    
    ! CALL GETBEADRODFORCE(CHAINP,BROWNFORCE,BRCRELAX,RFORCE,ENERGY)    
    ! DO B= 1,CHAINP%NPT
    !    print*, b, RFORCE(:,b)+brownforce(:,b)
    ! ENDDO
    ! ---------------------------
    
    ! --------test BD steps ------------
    CALL LANGEVINSTEPRK4(CHAINP,DELT,ENERGY,.TRUE.)
    print*, 'testx6:'
    do b = 1,chainp%npt
        print*, chainp%pos(:,b)
     enddo
     stop 1

    DEALLOCATE(BROWNFORCE,RFORCE)
    CALL CLEANUPCHAIN(CHAINP)
  END SUBROUTINE TESTBEADROD

  SUBROUTINE TESTCHAINEQUIL
    ! test equilibrium sampling of chain config
    USE CHAINUTIL
    USE KEYS
    USE SAMPLEUTIL
    IMPLICIT NONE
    TYPE(CHAIN), TARGET :: WLCLIST(1)
    TYPE(CHAIN), POINTER :: CHAINP
    INTEGER :: SC
    DOUBLE PRECISION :: DR(3), LASTCOORDS(6)

    CHAINP=>WLCLIST(1); 

    CALL READKEY

    CALL SETUPCHAIN(CHAINP,MAXNPT)
    CALL SETCHAINPARAMS(CHAINP)   
    CALL INITIALIZECHAIN(CHAINP,.TRUE.,INITRANGE)

    print*, 'chain info:', chainp%npt
    
    OPEN(UNIT=55,FILE=OUTFILE,STATUS='UNKNOWN')
    DO SC = 1,MCTOTSTEPS       
       CALL GETEQUILCHAIN(CHAINP,EQUILSAMPLETYPE,LASTCOORDS)
       DR = CHAINP%POS(:,CHAINP%NPT)-CHAINP%POS(:,1)
       IF (MOD(SC,MCPRINTFREQ).EQ.0) THEN
          PRINT*, SC, DR
       ENDIF
       WRITE(55,*) DR
    ENDDO
    CLOSE(55)
    !CALL OUTPUTSNAPSHOT(CHAINP,SNAPSHOTFILE,1,.FALSE.)

    CALL CLEANUPCHAIN(CHAINP)
  END SUBROUTINE TESTCHAINEQUIL

  SUBROUTINE TESTMCSAMPLEREL
    ! test monte carlo sampling of coupled relative coordinates
    USE KEYS
    USE SAMPLEUTIL
    IMPLICIT NONE
    DOUBLE PRECISION, ALLOCATABLE :: COORDS(:,:)
    DOUBLE PRECISION :: FACC, FINALRANGES(3)
    INTEGER :: C

    CALL READKEY

    ALLOCATE(COORDS(4,MAXNPT))

    IF (EQUILSAMPLETYPE.EQ.3) THEN
       !CALL MCSAMPLERRHOPHI(LS,LP,EPERP,-EC/LP,MAXNPT,MCSTATSTEPS,MCINITSTEPS,COORDS,FACC)
       CALL SAMPLERELCOORDSMC(LS,LP,GAM,EPAR,EPERP,-EC/LP,MAXNPT,MCSTATSTEPS,&
& MCINITSTEPS,COORDS,FINALRANGES)
    ELSEIF (EQUILSAMPLETYPE.EQ.2) THEN
       print*, 'not set up yet'
    ELSEIF (EQUILSAMPLETYPE.EQ.1) THEN
       CALL SAMPLERELCOORDS(LS,LP,GAM,EPAR,EPERP,-EC/LP,MAXNPT,COORDS)
    ENDIF

    OPEN(UNIT=55,FILE=OUTFILE,STATUS='UNKNOWN')
    DO C = 1,MAXNPT
       WRITE(55,*) COORDS(:,C)
    ENDDO
    CLOSE(55)

    !PRINT*, 'FINAL FACC:', FACC
    DEALLOCATE(COORDS)
  END SUBROUTINE TESTMCSAMPLEREL

  SUBROUTINE TESTSAMPLEUTILS
    ! test some sampling subroutines
    USE KEYS
    USE SAMPLEUTIL
    IMPLICIT NONE
    DOUBLE PRECISION :: RVAL, RHOVAL,PHIVAL,LORPARAM(3), A, B
    INTEGER :: C, ntry
    DOUBLE PRECISION, ALLOCATABLE :: COORDS(:,:)
    DOUBLE PRECISION :: AVGTRY

    CALL READKEY

    ! test sampling from cylindrical normal
    ! A = 0.1D0; B = 2D0
    ! CALL GETLORENTZENVELOPE(A,B,LORPARAM)
    ! PRINT*, 'PARAMETERS:', LORPARAM

    ! OPEN(UNIT=55,FILE=OUTFILE,STATUS='UNKNOWN')
    ! DO C = 1,NCHAIN
    !    IF (MOD(C,1000).EQ.0) PRINT*, C
    !    CALL SAMPLECYLNORMAL(A,B,LORPARAM,RVAL,NTRY)
    !    WRITE(55,*) RVAL, NTRY
    ! ENDDO
    ! CLOSE(55)

    ! test rho sampling
    ! OPEN(UNIT=55,FILE=OUTFILE,STATUS='UNKNOWN')
    ! DO C = 1,NCHAIN
    !    IF (MOD(C,1000).EQ.0) PRINT*, C
    !    CALL SAMPLERRHO(LS,LP,EPERP,-EC/LP,RVAL,RHOVAL)
    !    WRITE(55,*) RHOVAL
    ! ENDDO
    ! CLOSE(55)

    ! test full rejection sampling of r, rho, and phi
    ! OPEN(UNIT=55,FILE=OUTFILE,STATUS='UNKNOWN')
    ! DO C = 1,NCHAIN
    !    IF (MOD(C,1000).EQ.0) PRINT*, C
    !    CALL SAMPLERRHOPHI(LS,LP,EPERP,-EC/LP,RVAL,RHOVAL,PHIVAL,NTRY)
    !    WRITE(55,*) RVAL,RHOVAL,PHIVAL,NTRY
    ! ENDDO
    ! CLOSE(55)
    !PRINT*, RVAL, RHOVAL, PHIVAL, NTRY

    ! Test overall sampling of all relative coordinates
    ALLOCATE(COORDS(4,NCHAIN))
    CALL SAMPLERELCOORDS(LS,LP,GAM,EPAR,EPERP,-EC/LP,NCHAIN,COORDS)
    !COORDS(1:2,:) = COORDS(2:3,:)
    CALL SAMPLERRHOMVN(LS,LP,EPERP,-EC/LP,NCHAIN,COORDS(2,:),COORDS(3,:),AVGTRY)
    CALL SAMPLEPHICOND(NCHAIN,-EC/LS,COORDS(2,:),COORDS(3,:),COORDS(1,:),AVGTRY)
    PRINT*, 'avg try:', AVGTRY
    OPEN(UNIT=55,FILE=OUTFILE,STATUS='UNKNOWN')
    DO C = 1,NCHAIN
       WRITE(55,*) COORDS(:,C)
    ENDDO
    CLOSE(55)
    DEALLOCATE(COORDS)

  END SUBROUTINE TESTSAMPLEUTILS

  SUBROUTINE TESTANALYTIC
    ! test analytic calculations for chain dynamics
    ! starting with testing the force calculations for finely discretized chain
    USE CHAINUTIL
    USE KEYS
    USE BROWNDYN
    IMPLICIT NONE
    TYPE(CHAIN), TARGET :: WLCLIST(1)
    TYPE(OBSTACLE), TARGET :: OB
    TYPE(CHAIN), POINTER :: CHAINP
    TYPE(OBSTACLE), POINTER :: OBP
    DOUBLE PRECISION :: ENERGY, ENERGY0
    DOUBLE PRECISION, ALLOCATABLE :: RFORCE(:,:),RFORCETMP(:,:),UFORCE(:,:),UFORCETMP(:,:),BROWNPOS(:,:)
    DOUBLE PRECISION, PARAMETER :: TINY = 1D-7
    !    DOUBLE PRECISION :: OBSTPOS(3), OBSTRAD, OBSTMOD
    !    DOUBLE PRECISION :: OBFORCE(3),OBFORCETMP(3)
    INTEGER :: C, b, nread
    DOUBLE PRECISION :: DEL, TMPVEC(3)

    CHAINP=>WLCLIST(1); !OBP=>OB

    CALL READKEY

    CALL SETUPCHAIN(CHAINP,MAXNPT)
    CALL SETCHAINPARAMS(CHAINP)   
    CALL INITIALIZECHAIN(CHAINP,.TRUE.,INITRANGE)

    PRINT*, 'CHAIN INFO:', CHAINP%NPT, CHAINP%LS(1)

    CALL INPUTSNAPSHOT(WLCLIST,1,RESTARTFILE,0,NREAD)

    PRINT*, 'NREAD:', NREAD

    CALL OUTPUTSNAPSHOT(CHAINP,OUTFILE,1,.FALSE.)

    ALLOCATE(RFORCE(3,CHAINP%NPT),RFORCETMP(3,CHAINP%NPT),BROWNPOS(3,CHAINP%NPT))
    ALLOCATE(UFORCE(3,CHAINP%NPT),UFORCETMP(3,CHAINP%NPT))

    IF (CHAINP%STRETCHABLE.OR.CHAINP%SHEARABLE) THEN
       CALL GETCHAINFORCEINT(CHAINP,RFORCE,UFORCE,ENERGY0,.TRUE.)  
    ELSE
       print*, 'NOT SET UP FOR BEADRODFORCE YET'
       STOP 1
       !CALL GETBEADRODFORCE(CHAINP,BROWNPOS,RFORCE,ENERGY0)
    ENDIF

    B = 30; DEL = CHAINP%LS(1);
    print*, 'RFORCE:', RFORCE(:,B)
    print*, 'UFORCE:', UFORCE(:,B)
    PRINT*, 'UFORCE, U ANGLE:', DOT_PRODUCT(UFORCE(:,B),CHAINP%UVEC(:,B))/NORM(UFORCE(:,B))
    TMPVEC = UFORCE(:,B)-DOT_PRODUCT(UFORCE(:,B),CHAINP%UVEC(:,B))*CHAINP%UVEC(:,B)
    print*, 'projected uforce:', TMPVEC
    call CROSS_PRODUCT(UFORCE(:,B)/NORM(UFORCE(:,B)),CHAINP%UVEC(:,B),TMPVEC)
    PRINT*, 'CROSSPROD:', TMPVEC
    print*, 'testx1:'
    print*, chainp%uvec(:,b-1)
    print*, chainp%uvec(:,b)
    print*, chainp%uvec(:,b+1)
    print*, 'DU:', (CHAINP%UVEC(:,B+1)-CHAINP%UVEC(:,B-1))/(2*DEL), del, CHAINP%UVEC(:,B+1)-CHAINP%UVEC(:,B-1)
    print*, 'DR:', (CHAINP%POS(:,B+1)-CHAINP%POS(:,B-1))/(2*DEL)

    DEALLOCATE(RFORCE,RFORCETMP,UFORCE,UFORCETMP,BROWNPOS)
  END SUBROUTINE TESTANALYTIC

  SUBROUTINE TESTREADSNAP
    ! test reading in of snapshots
    USE CHAINUTIL
    USE KEYS
    USE BROWNDYN
    IMPLICIT NONE
    TYPE(CHAIN), ALLOCATABLE,TARGET :: WLCLIST(:)
    TYPE(CHAIN), POINTER :: CHAINP
    DOUBLE PRECISION :: ENERGY, ENERGY0, KT, DELT    
    DOUBLE PRECISION, PARAMETER :: TINY = 1D-6    
    INTEGER :: C, b, STEP,NREAD
      
   ! CHAINP=>WLC; !OBP=>OB
    CALL READKEY
    NCHAIN = 5

    ALLOCATE(WLCLIST(NCHAIN))
    DO C = 1,NCHAIN
       CHAINP=>WLCLIST(C)
       CALL SETUPCHAIN(CHAINP,MAXNPT)
       CALL SETCHAINPARAMS(CHAINP)
       CALL INITIALIZECHAIN(CHAINP,.TRUE.,INITRANGE)
    ENDDO

    CALL INPUTSNAPSHOT(WLCLIST,NCHAIN,SNAPSHOTFILE,1,NREAD)
    PRINT*, 'NREAD:', NREAD
    PRINT*, WLCLIST(1)%POS(:,1)
    PRINT*, WLCLIST(2)%UVEC(:,2)

    DO C = 1,NCHAIN
       CHAINP=>WLCLIST(C)
       CALL CLEANUPCHAIN(CHAINP)
    ENDDO
    DEALLOCATE(WLCLIST)
  END SUBROUTINE TESTREADSNAP
  
  SUBROUTINE TESTBROWNDYN
    ! test brownian dynamics step subroutine
    USE CHAINUTIL
    USE KEYS
    USE BROWNDYN
    IMPLICIT NONE
    TYPE(CHAIN), ALLOCATABLE,TARGET :: WLCLIST(:)
    TYPE(CHAIN), POINTER :: CHAINP
    DOUBLE PRECISION :: ENERGY, ENERGY0, KT, DELT
    DOUBLE PRECISION, ALLOCATABLE :: RFORCE(:,:),UFORCE(:,:)
    DOUBLE PRECISION, PARAMETER :: TINY = 1D-6

    INTEGER :: C, b, STEP
   
   ! CHAINP=>WLC; !OBP=>OB
    CALL READKEY
    ALLOCATE(WLCLIST(NCHAIN))
    DO C = 1,NCHAIN
       CHAINP=>WLCLIST(C)
       CALL SETUPCHAIN(CHAINP,MAXNPT)
       CALL SETCHAINPARAMS(CHAINP)
       CALL INITIALIZECHAIN(CHAINP,.TRUE.,INITRANGE)
    ENDDO

    CHAINP=>WLCLIST(1)
    ALLOCATE(RFORCE(3,CHAINP%NPT),UFORCE(3,CHAINP%NPT))

    CALL OUTPUTSNAPSHOT(CHAINP,'start.out',1,.FALSE.)
    CALL GETCHAINFORCEINT(CHAINP,RFORCE,UFORCE,ENERGY0,.TRUE.)
    print*, 'start energy:', ENERGY0

    KT = 1D0
    DELT = DELTSCL*KT/MAX(CHAINP%FRICTR,CHAINP%FRICTU)/(CHAINP%DELS(1))**2
    PRINT*, 'DELT:', DELT, outfile
    OPEN(99,FILE=OUTFILE,STATUS='UNKNOWN')
    write(99,*) NCHAIN
    DO STEP=1,10000
       DO C = 1,NCHAIN
          CHAINP=>WLCLIST(C)
          CALL LANGEVINSTEP(CHAINP,DELT,KT,ENERGY)
          CALL GETCHAINFORCEINT(CHAINP,RFORCE,UFORCE,ENERGY,.TRUE.)
          IF (MOD(STEP,100).EQ.0.AND.C.EQ.1) PRINT*, 'STEP, ENERGY:', STEP, ENERGY
          if (MOD(STEP,1).EQ.0) WRITE(99,*) STEP, C, ENERGY
       ENDDO
    ENDDO
    CLOSE(99)

    CHAINP=>WLCLIST(1)
    CALL OUTPUTSNAPSHOT(CHAINP,'finish.out',1,.FALSE.)
    CALL GETCHAINFORCEINT(CHAINP,RFORCE,UFORCE,ENERGY,.TRUE.)

    PRINT*, 'end energy:', ENERGY

    DEALLOCATE(RFORCE,UFORCE)
    DO C = 1,NCHAIN
       CHAINP=>WLCLIST(C)
       CALL CLEANUPCHAIN(CHAINP)
    ENDDO
    DEALLOCATE(WLCLIST)
  END SUBROUTINE TESTBROWNDYN

  SUBROUTINE TESTCHAINGROUP
    ! test setup for group of chains
    USE MANYCHAINS
    USE KEYS
    USE INPUTUTIL
    IMPLICIT NONE

    TYPE(CHAINGROUP),TARGET :: GROUP
    TYPE(CHAINGROUP), POINTER :: CGRP
    INTEGER :: NCON,NPT,NC,C, I,B, STARTSTEP
    DOUBLE PRECISION :: DIST, energy, SHEARMAT(3,3),DIFF(3)
    TYPE(CHAIN), POINTER :: CHAINP
    LOGICAL :: FILEEXISTS, SUCCESS

    CGRP=>GROUP

    CALL READKEY

    NCON = NCONNECT
    IF (SQUARELATTICE) THEN
       NCON = NCON + (NCHAIN/2)**2
    ELSEIF (DIAMONDLATTICE) THEN
       NCON = NCON + NDIAMOND(1)*NDIAMOND(2) + (NDIAMOND(1)+1)*(NDIAMOND(2)+1) - 4
    ENDIF

    CALL SETUPCHAINGROUP(CGRP,NCHAIN,NCON,NFORCE,MAXNPT)
    CALL SETCHAINGROUPPARAMS(CGRP)   

    NPT = CGRP%CHAINS(1)%NPT
    NC = NCHAIN/2

    IF (SQUARELATTICE) THEN
       DIST = (NPT-1)/(NC-1)*LS*gam       
       CALL INITIALIZESQUARELATTICE(CGRP,DIST)
    ELSEIF (DIAMONDLATTICE) THEN
       CALL INITIALIZEDIAMONDLATTICE(CGRP,LS*GAM,NDIAMOND,LENDIAMOND,WIDTHDIAMOND)
    ELSE
       DO C = 1,CGRP%NCHAIN
          CHAINP=>CGRP%CHAINS(C)
          CALL INITIALIZECHAIN(CHAINP,.FALSE.)
       ENDDO
    ENDIF

    ! apply shear deformation
    SHEARMAT = 0D0
    DO I = 1,3
       SHEARMAT(I,I) = 1D0
    ENDDO
    IF (SETSHEAR) THEN
       SHEARMAT(2,3)= SHEARGAMMA
    ENDIF
    CALL APPLYDEFORM(CGRP,SHEARMAT)

    STARTSTEP = 0
    IF (RESTART) THEN
       INQUIRE(FILE=RESTARTFILE,EXIST=FILEEXISTS) 
       IF (FILEEXISTS) THEN
          print*, 'Reading structure from file:', TRIM(ADJUSTL(RESTARTFILE))
          CALL READSNAPSHOTS(CGRP,RESTARTFILE,.FALSE.,STARTSTEP,SUCCESS)
          print*, 'Successfully read?:', SUCCESS, STARTSTEP
       ELSE
          PRINT*, 'WARNING: no restart file found!'
       ENDIF
    ENDIF

    

    PRINT*, 'CHAIN LENGTHS:'
    DO C = 1,CGRP%NCHAIN
       PRINT*, C, CGRP%CHAINS(C)%NPT
    ENDDO

    ! list all connections
    PRINT*, 'CONNECTIONS:'
    DO C = 1,CGRP%NCONNECT
       PRINT*, C, CGRP%CONNECT(C,:)
    ENDDO

    PRINT*, 'FIXED BEADS:'
    DO C = 1,CGRP%NCHAIN
       DO B = 1,CGRP%CHAINS(C)%NPT
          IF (ANY(CGRP%FIXBEAD(B,C,:))) THEN
             PRINT*, B,C, CGRP%FIXBEAD(B,C,:)
          ENDIF
       ENDDO
    ENDDO

    PRINT*, 'SEGMENT LENGTHS:'
    DO C = 1,CGRP%NCHAIN
       CHAINP=>CGRP%CHAINS(C)
       DO B = 2,CHAINP%NPT
          DIFF = CHAINP%POS(:,B)-CHAINP%POS(:,B-1)
          DIST = SQRT(DOT_PRODUCT(DIFF,DIFF))
          PRINT*, B,C,DIST
       ENDDO
    ENDDO

    ! output snapshot
    CALL GROUPSNAPSHOT(CGRP,SNAPSHOTFILE,0,.FALSE.)

    CALL GROUPENERGY(CGRP,ENERGY)
    PRINT*, 'ENERGY:', ENERGY

    CALL CLEANUPCHAINGROUP(CGRP)

  END SUBROUTINE TESTCHAINGROUP

  SUBROUTINE CHECKREDISC
    ! test rediscretization of chain

    USE CHAINUTIL
    USE KEYS
    USE REDISC
    USE GENUTIL
    USE INPUTUTIL
    IMPLICIT NONE
    TYPE(CHAIN), TARGET :: WLC
    TYPE(CHAIN), POINTER :: CHAINP
    INTEGER :: C, IND
    DOUBLE PRECISION :: ENERGY, DELE, NEWENERGY
    LOGICAL :: SUCCESS, FILEEXISTS
    INTEGER :: STARTSTEP
    DOUBLE PRECISION :: STARTENERGY, ENDENERGY
    DOUBLE PRECISION :: DIFF(3)
    INTEGER :: B

    CHAINP=>WLC

    CALL READKEY

    CALL SETUPCHAIN(CHAINP,MAXNPT)
    CALL SETCHAINPARAMS(CHAINP)
    CALL INITIALIZECHAIN(CHAINP,.TRUE.,INITRANGE)

    IF (RESTART) THEN
       INQUIRE(FILE=RESTARTFILE,EXIST=FILEEXISTS) 
       IF (FILEEXISTS) THEN
          print*, 'Reading structure from file:', TRIM(ADJUSTL(RESTARTFILE))
!          CALL READSNAPSHOTS(CHAINP,RESTARTFILE,STARTSTEP,SUCCESS)
          print*, 'Successfully read?:', SUCCESS, CHAINP%NPT
       ELSE
          PRINT*, 'WARNING: restart file not found'
       ENDIF
    ENDIF

    CALL OUTPUTSNAPSHOT(CHAINP,'start.out',0,.FALSE.)

    CALL READPARAMDATA('shearWLCparams.data')

    CALL GETENERGY(CHAINP,STARTENERGY)
    PRINT*, 'STARTING ENERGY:', STARTENERGY
    
     DO B = 1,CHAINP%NPT-1
       DIFF = CHAINP%POS(:,B+1)-CHAINP%POS(:,B)
       PRINT*, B, SQRT(DOT_PRODUCT(DIFF,DIFF)), CHAINP%LS(B), chainp%gam(b), chainp%beadenergy(b)
    ENDDO
    
    CALL REDISCADD(CHAINP,DELE)
  ! CALL REDISCREMOVe(CHAINP,DELE)
    print*, 'Number of beads:', CHAINP%NPT
    PRINT*, 'DELE:', DELE

    CALL GETENERGY(CHAINP,ENDENERGY)
    PRINT*, 'END ENERGY:', ENDENERGY, STARTENERGY+DELE
   
    
    CALL OUTPUTSNAPSHOT(CHAINP,'finish.out',0,.FALSE.)

    CALL CLEANUPCHAIN(CHAINP)
    CALL CLEANUPDATA
  END SUBROUTINE CHECKREDISC

    SUBROUTINE CHECKREMOVEADDBEAD
    ! test removing and adding of a bead
    USE CHAINUTIL
    USE KEYS
    USE REDISC
    USE GENUTIL
    USE INPUTUTIL
    IMPLICIT NONE
    TYPE(CHAIN), TARGET :: WLC
    TYPE(CHAIN), POINTER :: CHAINP
    INTEGER :: C, IND
    DOUBLE PRECISION :: ENERGY, DELE, NEWENERGY
    LOGICAL :: SUCCESS, FILEEXISTS
    INTEGER :: STARTSTEP
    DOUBLE PRECISION :: STARTENERGY, ENDENERGY, DIFF(3), ND

    CHAINP=>WLC

    CALL READKEY

    CALL SETUPCHAIN(CHAINP,MAXNPT)
    CALL SETCHAINPARAMS(CHAINP)
    CALL INITIALIZECHAIN(CHAINP,.false.,INITRANGE)

    CALL READPARAMDATA('shearWLCparams.data')

    IF (RESTART) THEN
       INQUIRE(FILE=RESTARTFILE,EXIST=FILEEXISTS) 
       IF (FILEEXISTS) THEN
          print*, 'Reading structure from file:', TRIM(ADJUSTL(RESTARTFILE))
!          CALL READSNAPSHOTS(CHAINP,RESTARTFILE,STARTSTEP,SUCCESS)
          print*, 'Successfully read?:', SUCCESS
       ELSE
          PRINT*, 'WARNING: restart file not found'
       ENDIF
    ENDIF
    print*, 'npt:', chainp%npt
    CALL OUTPUTSNAPSHOT(CHAINP,'start.out',0,.FALSE.)
    CALL GETENERGY(CHAINP,ENERGY)
    print*, 'initial energy:', ENERGY
    DO C = 1,CHAINP%NPT-1
       DIFF = CHAINP%POS(:,C+1)-CHAINP%POS(:,C)
       ND = SQRT(DOT_PRODUCT(DIFF,DIFF))
       PRINT*, C, chainp%beadenergy(c), chainp%lp(c)
    ENDDO
        
    CALL ADDBEAD(CHAINP,3,DELE)
    
    
    !CALL REMOVEBEAD(CHAINP,9,DELE)

    CALL GETENERGY(CHAINP,NEWENERGY)
    ! DO C = 1,CHAINP%NPT-1
    !    PRINT*, CHAINP%LS(C), CHAINP%LP(C), CHAINP%BEADENERGY(C+1)
    ! ENDDO

    PRINT*, 'DELE:', DELE
    PRINT*, ENERGY+DELE
    PRINT*, NEWENERGY, SUM(CHAINP%BEADENERGY(1:CHAINP%NPT))
    print*, 'npt:', chainp%npt
    CALL OUTPUTSNAPSHOT(CHAINP,'finish.out',0,.FALSE.)
  END SUBROUTINE CHECKREMOVEADDBEAD

   SUBROUTINE CHECKMINIMONTECARLO
    ! test single bead montecarlo
    USE CHAINUTIL
    USE KEYS
    USE REDISC
    USE GENUTIL
    USE INPUTUTIL
    IMPLICIT NONE
    TYPE(CHAIN), TARGET :: WLC
    TYPE(CHAIN), POINTER :: CHAINP
    INTEGER :: C, IND
    DOUBLE PRECISION :: ENERGY, DELE, NEWENERGY
    LOGICAL :: SUCCESS, FILEEXISTS
    INTEGER :: STARTSTEP
    DOUBLE PRECISION :: STARTENERGY, ENDENERGY, DIFF(3), ND

    CHAINP=>WLC

    CALL READKEY

    CALL SETUPCHAIN(CHAINP,MAXNPT)
    CALL SETCHAINPARAMS(CHAINP)
    CALL INITIALIZECHAIN(CHAINP,.false.,INITRANGE)

    CALL READPARAMDATA('shearWLCparams.data')

    IF (RESTART) THEN
       INQUIRE(FILE=RESTARTFILE,EXIST=FILEEXISTS) 
       IF (FILEEXISTS) THEN
          print*, 'Reading structure from file:', TRIM(ADJUSTL(RESTARTFILE))
!          CALL READSNAPSHOTS(CHAINP,RESTARTFILE,STARTSTEP,SUCCESS)
          print*, 'Successfully read?:', SUCCESS
       ELSE
          PRINT*, 'WARNING: restart file not found'
       ENDIF
    ENDIF
    print*, 'npt:', chainp%npt
    CALL OUTPUTSNAPSHOT(CHAINP,'start.out',0,.FALSE.)
    CALL GETENERGY(CHAINP,ENERGY)
    print*, 'initial energy:', ENERGY
    ! DO C = 1,CHAINP%NPT-1
    !    DIFF = CHAINP%POS(:,C+1)-CHAINP%POS(:,C)
    !    ND = SQRT(DOT_PRODUCT(DIFF,DIFF))
    !    PRINT*, C, CHAINP%ls(c), ND, CHAINP%POS(:,C)
    ! ENDDO
        
    CALL MINIMONTECARLO(CHAINP,3,1000)    
    

    CALL GETENERGY(CHAINP,NEWENERGY)
    ! DO C = 1,CHAINP%NPT-1
    !    PRINT*, CHAINP%LS(C), CHAINP%LP(C), CHAINP%BEADENERGY(C+1)
    ! ENDDO


    PRINT*, NEWENERGY

    CALL OUTPUTSNAPSHOT(CHAINP,'finish.out',0,.FALSE.)
  END SUBROUTINE CHECKMINIMONTECARLO


  SUBROUTINE CHECKCUBICROOT
    ! check cubic root finder
    USE REDISC
    IMPLICIT NONE
    DOUBLE PRECISION :: ROOT
    LOGICAL :: SUCCESS

    CALL CUBICROOT((/1D0,1D0,2D0,4D0/),(/-3D0,-1D0/),ROOT,SUCCESS)
    PRINT*, ROOT, SUCCESS
  END SUBROUTINE CHECKCUBICROOT

  SUBROUTINE CHECKSPHEREINTERSECT
    USE REDISC
    IMPLICIT NONE
    DOUBLE PRECISION :: PT1(3), PT2(3), PT3(3), RAD1, RAD2, RAD3
    LOGICAL :: INTERSECT

    ! check code for intersection of 3 spheres
    RAD1 = 1; RAD2 = 1; RAD3 = 1
    PT1 = (/0D0,0D0,0D0/)
    PT2 = (/0D0,0.5D0,0D0/)
    PT3 = (/0D0,0D0,0.5D0/)

    CALL INTERSECT3SPHERE(PT1,PT2,PT3,RAD1,RAD2,RAD3,INTERSECT)

    PRINT*, INTERSECT

  END SUBROUTINE CHECKSPHEREINTERSECT

  SUBROUTINE CHECKparamDATA
    ! test reading in of parameter data array
    ! and interpolation
    USE CHAINUTIL
    USE KEYS
    USE REDISC
    USE GENUTIL
    IMPLICIT NONE
    TYPE(CHAIN), TARGET :: WLC
    TYPE(CHAIN), POINTER :: CHAINP
    INTEGER :: C, IND
    DOUBLE PRECISION :: DEL, INTERP(6), FRAC

    CHAINP=>WLC

    CALL READKEY

    CALL SETUPCHAIN(CHAINP,MAXNPT)
    CALL SETCHAINPARAMS(CHAINP)

    CALL READPARAMDATA('shearWLCparams.data')

    DO C = 1,NPARAMDATA
       PRINT*, PARAMDATA(C,:)
    ENDDO

    ! Interpolate data
    DEL = 0.25D0
    CALL INTERPARRAY(PARAMDATA,(/NPARAMDATA,6/),1,DEL,IND,INTERP)
    PRINT*, IND
    PRINT*, INTERP
  END SUBROUTINE CHECKPARAMDATA

  SUBROUTINE TESTCHAINFORCE
    ! test calculation of chain forces
    USE CHAINUTIL
    USE KEYS
    USE BROWNDYN
    IMPLICIT NONE
    TYPE(CHAIN), TARGET :: WLC
    TYPE(OBSTACLE), TARGET :: OB
    TYPE(CHAIN), POINTER :: CHAINP
    TYPE(OBSTACLE), POINTER :: OBP
    DOUBLE PRECISION :: ENERGY, ENERGY0
    DOUBLE PRECISION, ALLOCATABLE :: RFORCE(:,:),RFORCETMP(:,:),UFORCE(:,:),UFORCETMP(:,:),BROWNPOS(:,:)
    DOUBLE PRECISION, PARAMETER :: TINY = 1D-7
    !    DOUBLE PRECISION :: OBSTPOS(3), OBSTRAD, OBSTMOD
    !    DOUBLE PRECISION :: OBFORCE(3),OBFORCETMP(3)
    INTEGER :: C, b

    CHAINP=>WLC; !OBP=>OB

    CALL READKEY

    CALL SETUPCHAIN(CHAINP,MAXNPT)
    CALL SETCHAINPARAMS(CHAINP)
    !CALL SETOBSTACLEPARAMS(OBP)
    CALL INITIALIZECHAIN(CHAINP,.TRUE.,INITRANGE)
    !OBP%COORDS = (/0.1D0,0.2D0,1D0/)

    print*, 'testx1:', chainp%npt
    !CALL OUTPUTSNAPSHOT(CHAINP,OUTFILE,1,.FALSE.)

    ALLOCATE(RFORCE(3,CHAINP%NPT),RFORCETMP(3,CHAINP%NPT),BROWNPOS(3,CHAINP%NPT))
    ALLOCATE(UFORCE(3,CHAINP%NPT),UFORCETMP(3,CHAINP%NPT))

    IF (CHAINP%STRETCHABLE.OR.CHAINP%SHEARABLE) THEN
       CALL GETCHAINFORCEINT(CHAINP,RFORCE,UFORCE,ENERGY0,.TRUE.)  
    ELSE
       print*, 'NOT SET UP FOR BEADRODFORCE YET'
       STOP 1
       !CALL GETBEADRODFORCE(CHAINP,BROWNPOS,RFORCE,ENERGY0)
    ENDIF
    !CALL GETENERGY(CHAINP,ENERGY0)
    !   PRINT*, ENERGY0, ENERGY

    !CALL GETOBSTACLEFORCE(CHAINP,OBP,FORCES,OBFORCE,ENERGY0)
    !CALL GETCHAINFORCEGAUSS(CHAINP,RFORCE,ENERGY0)

    DO B = 1,CHAINP%NPT
       DO C = 1,3
          CHAINP%POS(C,B) = CHAINP%POS(C,B) + TINY
          IF (CHAINP%STRETCHABLE.OR.CHAINP%SHEARABLE) THEN
             CALL GETCHAINFORCEINT(CHAINP,RFORCETMP,UFORCETMP,ENERGY,.TRUE.)  
          ELSE
             print*, 'NOT SET UP FOR BEADRODFORCE YET'
             STOP 1
             !CALL GETBEADRODFORCE(CHAINP,BROWNPOS,RFORCETMP,ENERGY)
          ENDIF
          !CALL GETCHAINFORCEINT(CHAINP,RFORCETMP,UFORCETMP,ENERGY)
          !CALL GETENERGY(CHAINP,ENERGY)
          !CALL GETOBSTACLEFORCE(CHAINP,OBP,FORCESTMP,OBFORCETMP,ENERGY)
          !CALL GETCHAINFORCEGAUSS(CHAINP,RFORCETMP,ENERGY)
          PRINT*, 'POS', B, C, RFORCE(C,B), (ENERGY-ENERGY0)/TINY
          CHAINP%POS(C,B) = CHAINP%POS(C,B) - TINY
       ENDDO
       ! DO C = 1,3
       !    CHAINP%UVEC(C,B) = CHAINP%UVEC(C,B) + TINY
       !    CALL GETCHAINFORCEINT(CHAINP,RFORCETMP,UFORCETMP,ENERGY)
       !    CALL GETENERGY(CHAINP,ENERGY)
       !    !CALL GETOBSTACLEFORCE(CHAINP,OBP,FORCESTMP,OBFORCETMP,ENERGY)
       !    PRINT*, 'UVEC', B, C, UFORCE(C,B), (ENERGY-ENERGY0)/TINY
       !    CHAINP%UVEC(C,B) = CHAINP%UVEC(C,B) - TINY
       ! ENDDO
    END DO

    ! DO B = 1,CHAINP%NPT
    !    PRINT*, 'U * UFORCE:', B, DOT_PRODUCT(UFORCE(:,B),CHAINP%UVEC(:,B))
    ! ENDDO

    ! PRINT*, 'FORCE ON OBSTACLE:'
    ! DO C = 1,3
    !    OBP%COORDS(C) = OBP%COORDS(C) + TINY
    !    CALL GETOBSTACLEFORCE(CHAINP,OBP,FORCESTMP,OBFORCETMP,ENERGY)
    !    PRINT*, C, OBFORCE(C), (ENERGY-ENERGY0)/TINY
    !    OBP%COORDS(C) = OBP%COORDS(C) - TINY
    ! END DO

    !CALL OUTPUTCHAINOBST(CHAINP,OBP,'test.out')

    DEALLOCATE(RFORCE,RFORCETMP,UFORCE,UFORCETMP,BROWNPOS)
  END SUBROUTINE TESTCHAINFORCE

  SUBROUTINE TESTCRANK
    USE CHAINUTIL
    USE KEYS
    USE MONTECARLO
    USE SAMPLEUTIL
    IMPLICIT NONE

    TYPE(CHAIN), TARGET :: WLC
    TYPE(CHAIN), POINTER :: CHAINP
    DOUBLE PRECISION :: ENERGY, DELE
    INTEGER :: B, BPIVOT(2)
    DOUBLE PRECISION :: lastcoords(6), ENERGYOLD
    DOUBLE PRECISION, ALLOCATABLE :: PREVPOS(:,:)
    INTEGER :: CHOOSEB, CT, NFLEXBEAD

    CHAINP=>WLC

    CALL READKEY

    CALL SETUPCHAIN(CHAINP,MAXNPT)
    CALL SETCHAINPARAMS(CHAINP)
    CALL INITIALIZECHAIN(CHAINP,.TRUE.,INITRANGE(2))
    CALL GETEQUILCHAIN(CHAINP,EQUILSAMPLETYPE,LASTCOORDS)
    ! PRINT*, CHAINP%POS

    ALLOCATE(PREVPOS(3,CHAINP%NPT))

    PREVPOS = CHAINP%POS
    CALL GETENERGY(CHAINP,ENERGY)
    print*, 'initial energy:,', energy
    ! DO B = 1,NPT
    !    PRINT*, 'BEAD ENERGY:', B, CHAINP%BEADENERGY(B)
    ! ENDDO
    ! PRINT*, 'FORCE ENERGY:', CHAINP%FORCEENERGY

    BPIVOT = 0
    !CALL CRANKMOVE1(CHAINP,2*PI,1D0,BPIVOT(1),DELE)
    !CALL CRANKMOVE2(CHAINP,2*PI,1D0,BPIVOT(1),DELE)

    NFLEXBEAD = CHAINP%NPT - CHAINP%NFIXBEAD
    ! choose which moveable bead to move
    ! WARNING: this is really inefficient...
    CHOOSEB = FLOOR(GRND()*NFLEXBEAD)+1
    CT=0
    DO B = 1,CHAINP%NPT
       IF (.NOT.CHAINP%ISFIXED(B)) THEN
          CT = CT + 1
       ENDIF
       IF (CT.EQ.CHOOSEB) THEN
          BPIVOT(1) = B
          EXIT
       ENDIF
    ENDDO

    CALL LOCALMOVE(CHAINP,INITRANGE(1),INITRANGE(2),BPIVOT(1),.TRUE.,.TRUE.)  
    ENERGYOLD = CHAINP%BEADENERGY(BPIVOT(1))+CHAINP%BEADENERGY(BPIVOT(1)+1)
    CALL GETBEADENERGY(CHAINP,BPIVOT(1),CHAINP%BEADENERGY(BPIVOT(1)))
    CALL GETBEADENERGY(CHAINP,BPIVOT(1)+1,CHAINP%BEADENERGY(BPIVOT(1)+1))
    DELE = CHAINP%BEADENERGY(BPIVOT(1))+CHAINP%BEADENERGY(BPIVOT(1)+1) - ENERGYOLD

    PRINT*, ENERGY+DELE

    PRINT*, BPIVOT

    DO B = 1,CHAINP%NPT
       PRINT*, B, CHAINP%POS(:,B)-PREVPOS(:,B)
    ENDDO

    CALL GETENERGY(CHAINP,ENERGY)
    PRINT*, 'NEW ENERGY', ENERGY
    ! DO B = 1,NPT
    !    PRINT*, 'BEAD ENERGY:', B, CHAINP%BEADENERGY(B)
    ! ENDDO
    ! PRINT*, 'FORCE ENERGY:', CHAINP%FORCEENERGY
    CALL CLEANUPCHAIN(CHAINP)
    DEALLOCATE(PREVPOS)

  END SUBROUTINE TESTCRANK
END PROGRAM MAIN
