!!  Copyright (C) Stichting Deltares, 2005-2020.
!!
!!  This file is part of iMOD.
!!
!!  This program is free software: you can redistribute it and/or modify
!!  it under the terms of the GNU General Public License as published by
!!  the Free Software Foundation, either version 3 of the License, or
!!  (at your option) any later version.
!!
!!  This program is distributed in the hope that it will be useful,
!!  but WITHOUT ANY WARRANTY; without even the implied warranty of
!!  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
!!  GNU General Public License for more details.
!!
!!  You should have received a copy of the GNU General Public License
!!  along with this program.  If not, see <http://www.gnu.org/licenses/>.
!!
!!  Contact: imod.support@deltares.nl
!!  Stichting Deltares
!!  P.O. Box 177
!!  2600 MH Delft, The Netherlands.
!!
MODULE MOD_INTERSECT

USE MOD_POLINT, ONLY : POL1LOCATE
USE MOD_INTERSECT_PAR
USE MOD_QKSORT

CONTAINS

 !###======================================================================
 SUBROUTINE INTERSECT_EQUI(XMIN,XMAX,YMIN,YMAX,CSX_IN,CSY_IN,XIN1,XIN2,YIN1,YIN2,N,LHFB)  
 !###======================================================================
 IMPLICIT NONE
 REAL(KIND=DP_KIND),INTENT(IN) :: XMIN,XMAX,YMIN,YMAX,CSX_IN,CSY_IN
 REAL(KIND=DP_KIND),INTENT(IN) :: XIN1,XIN2,YIN1,YIN2
 INTEGER,INTENT(INOUT) :: N
 LOGICAL,INTENT(IN) :: LHFB
 REAL(KIND=DP_KIND) :: X,Y,XMN,XMX,YMN,YMX,DX,DY,LENG,TD,X1,X2,Y1,Y2,CSX,CSY,A,B
 INTEGER :: I,ICOL,IROW,ID,N_IN
 LOGICAL :: LFLIP
 
 X1=XIN1; Y1=YIN1; X2=XIN2; Y2=YIN2
 
 IF(.NOT.ASSOCIATED(XA))ALLOCATE(XA(1000)); IF(.NOT.ASSOCIATED(YA))ALLOCATE(YA(1000))
 IF(.NOT.ASSOCIATED(FA))ALLOCATE(FA(1000)); IF(.NOT.ASSOCIATED(LN))ALLOCATE(LN(1000))
 IF(.NOT.ASSOCIATED(CA))ALLOCATE(CA(1000)); IF(.NOT.ASSOCIATED(RA))ALLOCATE(RA(1000))

 CSX=CSX_IN
 CSY=CSY_IN
 IF(LHFB)THEN
  CSX=CSX/2.0D0
  CSY=CSY/2.0D0
 ENDIF
 
 N_IN=N

 IF(.NOT.INTERSECT_EQUATION(X1,X2,Y1,Y2,N,A,B,LFLIP))RETURN
 
 !## find search box - result can be negative, does not matter!
 IF(X1.LE.XMIN)THEN
  XMN=XMIN
 ELSE 
!  I=0 !1
  XMN=XMIN+CSX*(INT((X1-XMIN)/CSX)) !+I)
 ENDIF
 IF(X2.GE.XMAX)THEN
  XMX=XMAX
 ELSE 
!  I=1 !0
!  DX=X2-XMIN
!  IF(MOD(DX,CSX).EQ.0.0D0)I=-1
  XMX=XMIN+CSX*(INT((X2-XMIN)/CSX)) !+I) 
 ENDIF
 
 Y=MIN(Y1,Y2)
 IF(Y.LE.YMIN)THEN
  YMN=YMIN
 ELSE
  I =0
  DY=YMAX-Y
  IF(MOD(DY,CSY).EQ.0.0D0)I=-1
  YMN=YMAX-CSY*(INT((YMAX-Y)/CSY)+I)
 ENDIF
 Y=MAX(Y1,Y2)
 IF(Y.GE.YMAX)THEN
  YMX=Y
 ELSE
  I =1
  YMX=YMAX-CSY*(INT((YMAX-Y)/CSY)+I)
 ENDIF
 
 !## not for horizontal lines
 IF(IHOR.EQ.0)THEN
 
  !## continue seach rest of intersections
  !## try intersections with x-axes firstly
  Y=YMN-CSY
  DO
   Y=Y+CSY
   IF(Y.GT.YMX)EXIT

   !## array overwritten
   N=N+1; CALL INTERSECT_RESIZEVECTORS(N) 

   IF(IVER.EQ.1)THEN
    XA(N)=X1 !## same as xmx
   ELSE
    XA(N)=(Y-B)/A
   ENDIF
   YA(N)=Y
 
  ENDDO

 ENDIF

 !## not for vertical lines
 IF(IVER.EQ.0)THEN 

  !## try intersections with y-axes secondly
  X=XMN-CSX
  DO
   X=X+CSX
   IF(X.GT.XMX)EXIT

   !## array overwritten
   N=N+1; CALL INTERSECT_RESIZEVECTORS(N) 
   XA(N)=X
   IF(IHOR.EQ.1)THEN
    YA(N)=Y1  !## same as ymx
   ELSE
    YA(N)=A*X+B
   ENDIF
 
  ENDDO
 
 ENDIF
 
 CSX=CSX_IN
 CSY=CSY_IN

 DX=X1-X2; DY=Y2-Y1
 CALL INTERSECT_SORT(DX,DY,N_IN+1,N,LFLIP)
 
 !## sample each of the point to determine irow/icol, overwrite point with this
 !## skip first and last, they represented already by the second and one-last point
 XMN=MIN(X1,X2); XMX=MAX(X1,X2)
 YMN=MIN(Y1,Y2); YMX=MAX(Y1,Y2)
 DO I=N_IN+2,N 

  !## mid point
  X   =(XA(I-1)+XA(I))/2.0D0
  Y   =(YA(I-1)+YA(I))/2.0D0
  
  IF(LHFB)THEN
   ICOL=0; IF((X.GT.MIN(X1,X2).AND.X.LT.MAX(X1,X2)).OR.X1.EQ.X2)ICOL=INT((X-XMIN)/CSX)+1
   IROW=0; IF((Y.GT.MIN(Y1,Y2).AND.Y.LT.MAX(Y1,Y2)).OR.Y1.EQ.Y2)IROW=INT((YMAX-Y)/CSY)+1
  ELSE
   ICOL=0
!   IF(X.EQ.XMN.OR.X.EQ.XMX)THEN
!    ICOL=INT((X-XMIN)/CSX)+1
!   ELSE
   IF(X.GE.XMN.AND.X.LE.XMX)THEN
    ICOL=INT((X-XMIN)/CSX)+1
   ENDIF
   IROW=0
!   IF(Y.EQ.YMN.OR.Y.EQ.YMX)THEN
!    IROW=INT((YMAX-Y)/CSY)
!   ELSE
   IF(Y.GE.YMN.AND.Y.LE.YMX)THEN
    IROW=INT((YMAX-Y)/CSY)+1
   ENDIF
  ENDIF

  TD=CSX*CSY; ID=0 !## fraction
  IF(ICOL.GT.0.AND.IROW.GT.0)THEN
   CALL INTERSECT_NCORNER(ID,TD,XMIN+(ICOL-1)*CSX,YMAX-(IROW-1)*CSY,X,Y,1)
   CALL INTERSECT_NCORNER(ID,TD,XMIN+ ICOL   *CSX,YMAX-(IROW-1)*CSY,X,Y,2)
   CALL INTERSECT_NCORNER(ID,TD,XMIN+ ICOL   *CSX,YMAX- IROW   *CSY,X,Y,3)
   CALL INTERSECT_NCORNER(ID,TD,XMIN+(ICOL-1)*CSX,YMAX- IROW   *CSY,X,Y,4)
  ENDIF
  
  FA(I-1)=DBLE(ID) 
  
  DX  =XA(I)-XA(I-1)
  DY  =YA(I)-YA(I-1)
  LENG=DX**2.0D0+DY**2.0D0; IF(LENG.GT.0.0D0)LENG=SQRT(LENG)

  !## store results in row/column indices
  CA(I-1)=DBLE(ICOL)
  RA(I-1)=DBLE(IROW)
  LN(I-1)=LENG
  
 END DO
 N=N-1

 END SUBROUTINE INTERSECT_EQUI
 
 !###======================================================================
 SUBROUTINE INTERSECT_NCORNER(ID,TD,X1,Y1,XC,YC,JD)
 !###======================================================================
 IMPLICIT NONE
 INTEGER,INTENT(INOUT) :: ID
 REAL(KIND=DP_KIND),INTENT(INOUT) :: TD
 INTEGER,INTENT(IN) :: JD
 REAL(KIND=DP_KIND),INTENT(IN) :: X1,Y1,XC,YC
 REAL(KIND=DP_KIND) :: D
  
 !## get nearest corner
 D=(XC-X1)**2.0D0+(YC-Y1)**2.0D0
 IF(D.GT.0.0D0)THEN
  D=SQRT(D)
  IF(D.LT.TD)THEN
   ID=JD
   TD=D
  ENDIF
 ELSE
  ID=JD
  TD=0.0D0
 ENDIF

 END SUBROUTINE INTERSECT_NCORNER
 
 !###======================================================================
 SUBROUTINE INTERSECT_NONEQUI(DELR,DELC,NROW,NCOL,XIN1,XIN2,YIN1,YIN2,N,LHFB)
 !###======================================================================
 IMPLICIT NONE
 INTEGER,INTENT(IN) :: NROW,NCOL
 REAL(KIND=DP_KIND),INTENT(IN),DIMENSION(0:NCOL) :: DELR
 REAL(KIND=DP_KIND),INTENT(IN),DIMENSION(0:NROW) :: DELC
 REAL(KIND=DP_KIND),INTENT(IN) :: XIN1,XIN2,YIN1,YIN2
 INTEGER,INTENT(INOUT) :: N
 LOGICAL,INTENT(IN) :: LHFB
 REAL(KIND=DP_KIND) :: X,Y,XMN,XMX,YMN,YMX,DX,DY,LENG,XMIN,YMIN,XMAX,YMAX,X1,X2,Y1,Y2,CS,TD,CSX,CSY,A,B
 INTEGER :: I,ICOL,IROW,IMN,JMN,ID,N_IN
 LOGICAL :: LFLIP
 
 X1=XIN1; Y1=YIN1; X2=XIN2; Y2=YIN2
 
 IF(.NOT.ASSOCIATED(XA))ALLOCATE(XA(1000)); IF(.NOT.ASSOCIATED(YA))ALLOCATE(YA(1000))
 IF(.NOT.ASSOCIATED(FA))ALLOCATE(FA(1000)); IF(.NOT.ASSOCIATED(LN))ALLOCATE(LN(1000))
 IF(.NOT.ASSOCIATED(CA))ALLOCATE(CA(1000)); IF(.NOT.ASSOCIATED(RA))ALLOCATE(RA(1000))

 XMIN=DELR(0)
 XMAX=DELR(NCOL)
 YMIN=DELC(NROW)
 YMAX=DELC(0)

 N_IN=N
 
 IF(.NOT.INTERSECT_EQUATION(X1,X2,Y1,Y2,N,A,B,LFLIP))RETURN
 
 !## continue search rest of intersections
 !## try intersections with y-axes firstly
 IMN=0; DO I=1,NROW 
  IF(DELC(I).GT.MIN(Y1,Y2))THEN; YMN=DELC(I); IMN=I; ENDIF
 END DO
 JMN=0; DO I=NROW,1,-1 
  IF(DELC(I).LT.MAX(Y1,Y2))THEN; YMX=DELC(I); JMN=I; ENDIF
 END DO

 IF(IMN.GT.0.AND.JMN.GT.0)THEN

  !## not for horizontal lines
  IF(IHOR.EQ.0)THEN

   CS=DELC(IMN-1)-DELC(IMN)
   IF(LHFB)CS=CS/2.0D0
   Y =YMN-CS
   DO

    Y=Y+CS
    IF(Y.GT.YMX)EXIT
    N=N+1; CALL INTERSECT_RESIZEVECTORS(N) 
    IF(IVER.EQ.1)THEN
     XA(N)=X1 !## same as xmx
    ELSE
     XA(N)=(Y-B)/A
    ENDIF
    YA(N)=Y

    !## double intersections, for better estimate for hfb
    IF(LHFB)THEN
     Y=Y+CS
     !## array overwritten
     N=N+1; CALL INTERSECT_RESIZEVECTORS(N)
     IF(IVER.EQ.1)THEN
      XA(N)=X1 !## same as xmx
     ELSE
      XA(N)=(Y-B)/A
     ENDIF
     YA(N)=Y
    ENDIF
  
    IMN  =IMN-1
    !## model is not bigger than line-segment
    IF(IMN.LE.0)EXIT
    CS=DELC(IMN)-DELC(IMN+1)
    IF(LHFB)CS=CS/2.0D0
   ENDDO

  ENDIF
 ENDIF

 !## try intersections with x-axes secondly
 IMN=0; DO I=NCOL,1,-1 
  IF(DELR(I).GT.X1)THEN; XMN=DELR(I); IMN=I; ENDIF
 END DO
 JMN=0; DO I=0,NCOL
  IF(DELR(I).LT.X2)THEN; XMX=DELR(I); JMN=I; ENDIF
 END DO

 IF(IMN.GT.0.AND.JMN.GT.0)THEN

  !## not for vertical lines
  IF(IVER.EQ.0)THEN

   CS=DELR(IMN-1)-DELR(IMN)
   IF(LHFB)CS=CS/2.0D0
   X =XMN-CS
   DO
   
    X=X+CS
    IF(X.GT.XMX)EXIT
    N=N+1; CALL INTERSECT_RESIZEVECTORS(N) 
    XA(N)=X
    IF(IHOR.EQ.1)THEN
     YA(N)=Y1  !## same as ymx
    ELSE
     YA(N)=A*X+B
    ENDIF

    !## double intersections, for better estimate for hfb
    IF(LHFB)THEN
     X=X+CS
     !## array overwritten
     N=N+1; CALL INTERSECT_RESIZEVECTORS(N) 
     XA(N)=X
     IF(IHOR.EQ.1)THEN
      YA(N)=Y1  !## same as ymx
     ELSE
      YA(N)=A*X+B
     ENDIF
    ENDIF
  
    IMN  =IMN+1
    !## model is not bigger than line-segment
    IF(IMN.GT.NCOL)EXIT
    CS=DELR(IMN)-DELR(IMN-1)
    IF(LHFB)CS=CS/2.0D0
   ENDDO

  ENDIF
 ENDIF

 !## sort intersections, determined by the one with the largest difference
 DX=X1-X2; DY=Y2-Y1
 CALL INTERSECT_SORT(DX,DY,N_IN+1,N,LFLIP)

 !## sample each of the point to determine irow/icol, overwrite point with this
 !## skip first and last, they represented already by the second and one-last point
 XMN=MIN(X1,X2); XMX=MAX(X1,X2)
 YMN=MIN(Y1,Y2); YMX=MAX(Y1,Y2)
 DO I=N_IN+2,N
  !## mid point
  X   =(XA(I-1)+XA(I))/2.0D0
  Y   =(YA(I-1)+YA(I))/2.0D0
  
  IF(LHFB)THEN
   ICOL=0; IF((X.GT.MIN(X1,X2).AND.X.LT.MAX(X1,X2)).OR.X1.EQ.X2)CALL POL1LOCATE(DELR,NCOL+1,DBLE(X),ICOL)
   IROW=0; IF((Y.GT.MIN(Y1,Y2).AND.Y.LT.MAX(Y1,Y2)).OR.Y1.EQ.Y2)CALL POL1LOCATE(DELC,NROW+1,DBLE(Y),IROW)
  ELSE
   ICOL=0
   IF(X.EQ.XMN)THEN
    ICOL=1
   ELSEIF(X.EQ.XMX)THEN
    ICOL=NCOL
   ELSEIF(X.GT.XMN.AND.X.LT.XMX)THEN
    CALL POL1LOCATE(DELR,NCOL+1,X,ICOL)
   ENDIF
   IROW=0
   IF(Y.EQ.YMN)THEN
    IROW=NROW
   ELSEIF(Y.EQ.YMX)THEN
    IROW=1
   ELSEIF(Y.GT.YMN.AND.Y.LT.YMX)THEN
    CALL POL1LOCATE(DELC,NROW+1,Y,IROW)
   ENDIF
  ENDIF

  ID=0 !## fraction

  IF(ICOL.GE.1.AND.ICOL.LE.NCOL.AND. &
     IROW.GE.1.AND.IROW.LE.NROW)THEN
   CSX=DELR(ICOL)-DELR(ICOL-1)
   CSY=DELC(IROW-1)-DELC(IROW)
   TD=CSX*CSY
   CALL INTERSECT_NCORNER(ID,TD,DELR(ICOL-1),DELC(IROW-1),X,Y,1)
   CALL INTERSECT_NCORNER(ID,TD,DELR(ICOL)  ,DELC(IROW-1),X,Y,2)
   CALL INTERSECT_NCORNER(ID,TD,DELR(ICOL)  ,DELC(IROW)  ,X,Y,3)
   CALL INTERSECT_NCORNER(ID,TD,DELR(ICOL-1),DELC(IROW)  ,X,Y,4)
  ELSE
   ICOL=0; IROW=0
  ENDIF
  
  FA(I-1)=DBLE(ID) 

  DX  =XA(I)-XA(I-1)
  DY  =YA(I)-YA(I-1)
  LENG=DX**2.0D0+DY**2.0D0; IF(LENG.GT.0.0D0)LENG=SQRT(LENG)

  !## store results in row/column indices
  CA(I-1)=DBLE(ICOL)
  RA(I-1)=DBLE(IROW)

  LN(I-1)=LENG
 END DO
 N=N-1

 END SUBROUTINE INTERSECT_NONEQUI

 !###======================================================================
 SUBROUTINE INTERSECT_NULLIFY()
 !###======================================================================
 IMPLICIT NONE
 
 NULLIFY(XA,YA,FA,LN,CA,RA)
  
 END SUBROUTINE INTERSECT_NULLIFY

 !###======================================================================
 SUBROUTINE INTERSECT_DEALLOCATE()
 !###======================================================================
 IMPLICIT NONE
 
 IF(ASSOCIATED(XA))DEALLOCATE(XA)
 IF(ASSOCIATED(YA))DEALLOCATE(YA) 
 IF(ASSOCIATED(FA))DEALLOCATE(FA) 
 IF(ASSOCIATED(LN))DEALLOCATE(LN)
 IF(ASSOCIATED(CA))DEALLOCATE(CA)
 IF(ASSOCIATED(RA))DEALLOCATE(RA)
  
 END SUBROUTINE INTERSECT_DEALLOCATE
 
 !###======================================================================
 SUBROUTINE INTERSECT_RESIZEVECTORS(N)
 !###======================================================================
 IMPLICIT NONE
 INTEGER,PARAMETER :: DN=100
 INTEGER,INTENT(IN) :: N
 INTEGER :: MM,NN,I
 
 IF(N.LE.SIZE(XA))RETURN
 
 NN=SIZE(XA)
 MM=MAX(N,NN+DN)
 
 ALLOCATE(XA_DUMMY(MM),YA_DUMMY(MM),FA_DUMMY(MM),LN_DUMMY(MM),CA_DUMMY(MM),RA_DUMMY(MM))

 DO I=1,NN
  XA_DUMMY(I)=XA(I)
  YA_DUMMY(I)=YA(I)
  FA_DUMMY(I)=FA(I)
  LN_DUMMY(I)=LN(I)
  CA_DUMMY(I)=CA(I)
  RA_DUMMY(I)=RA(I)
 ENDDO
 
 DEALLOCATE(XA,YA,FA,LN,CA,RA)
 
 XA=>XA_DUMMY
 YA=>YA_DUMMY
 FA=>FA_DUMMY
 LN=>LN_DUMMY
 CA=>CA_DUMMY
 RA=>RA_DUMMY
 
 END SUBROUTINE INTERSECT_RESIZEVECTORS
 
 !###======================================================================
 LOGICAL FUNCTION INTERSECT_EQUATION(X1,X2,Y1,Y2,N,A,B,LFLIP)
 !###======================================================================
 IMPLICIT NONE
 REAL(KIND=DP_KIND),INTENT(INOUT) :: X1,X2,Y1,Y2,A,B
 LOGICAL,INTENT(OUT) :: LFLIP
 INTEGER,INTENT(INOUT) :: N
 REAL(KIND=DP_KIND) :: X,Y,DX,DY

 INTERSECT_EQUATION=.FALSE.

! XBEGIN=X1; YBEGIN=Y1

 !## arrange x1,x2,y1,y2 such that x1<x2
 LFLIP=.FALSE.
 IF(X1.GT.X2)THEN
  LFLIP=.TRUE.
  X =X1; Y =Y1
  X1=X2; Y1=Y2
  X2=X;  Y2=Y
 ENDIF

 !## use always mid between point x1,y1 and x2,y2 as first position
 N=N+1; CALL INTERSECT_RESIZEVECTORS(N) 
 XA(N)= X1; YA(N)= Y1
 N=N+1; CALL INTERSECT_RESIZEVECTORS(N) 
 XA(N)= X1; YA(N)= Y1

 N=N+1; CALL INTERSECT_RESIZEVECTORS(N) 
 XA(N)= (X1+X2)/2.0; YA(N)= (Y1+Y2)/2.0

 N=N+1; CALL INTERSECT_RESIZEVECTORS(N) 
 XA(N)= X2; YA(N)= Y2
 N=N+1; CALL INTERSECT_RESIZEVECTORS(N) 
 XA(N)= X2; YA(N)= Y2

 !## find mathematical expression for line: y=ax+b
 DX=X2-X1; DY=Y2-Y1
 IVER=0; IHOR=0
 IF(DX.EQ.0.0D0)IVER=1
 IF(DY.EQ.0.0D0)IHOR=1
 IF(DX.NE.0.0D0)THEN 
  A=DY/DX
  B=Y1-A*X1
 ENDIF

 INTERSECT_EQUATION=.TRUE.

 END FUNCTION INTERSECT_EQUATION

 !###======================================================================
 SUBROUTINE INTERSECT_SORT(DX,DY,N_IN,N,LFLIP)
 !###======================================================================
 IMPLICIT NONE
 REAL(KIND=DP_KIND),INTENT(IN) :: DX,DY
 LOGICAL,INTENT(INOUT) :: LFLIP
 INTEGER,INTENT(IN) :: N,N_IN
 REAL(KIND=DP_KIND) :: X,Y,DX1,DX2,DY1,DY2,LENG
 INTEGER :: I

 !## sort intersections, determined by the one with the largest difference
 IF(ABS(DX).GE.ABS(DY))THEN
  DX1=XA(N_IN)-XA(N_IN+(N-N_IN)) 
  CALL QKSORT((N-N_IN)+1,XA(N_IN:),V2=YA(N_IN:))
  DX2=XA(N_IN)-XA(N_IN+(N-N_IN)) 
  IF(DX1.LT.0.0D0.AND.DX2.GT.0.0D0)LFLIP=.NOT.LFLIP
  IF(DX1.GT.0.0D0.AND.DX2.LT.0.0D0)LFLIP=.NOT.LFLIP
 ELSE
  DY1=YA(N_IN)-YA(N_IN+(N-N_IN)) 
  CALL QKSORT((N-N_IN)+1,YA(N_IN:),V2=XA(N_IN:))
  DY2=YA(N_IN)-YA(N_IN+(N-N_IN)) 
  IF(DY1.LT.0.0D0.AND.DY2.GT.0.0D0)LFLIP=.NOT.LFLIP
  IF(DY1.GT.0.0D0.AND.DY2.LT.0.0D0)LFLIP=.NOT.LFLIP
 ENDIF

 !## resort - if neccessary
 IF(LFLIP)THEN 
  DO I=N_IN,N_IN+((N-N_IN)/2)
   X           =XA(I)
   XA(I)       =XA(N-I+N_IN) 
   XA(N-I+N_IN)=X
   Y           =YA(I)
   YA(I)       =YA(N-I+N_IN) 
   YA(N-I+N_IN)=Y
   LENG        =LN(I)
   LN(I)       =LN(N-I+N_IN)
   LN(N-I+N_IN)=LENG
  END DO
 ENDIF

 END SUBROUTINE INTERSECT_SORT

END MODULE