!
!  match    finding of cross identification between two lists
!           the core of the matching algorithm
!
!  Copyright © 2010 - 2015, 2017-8 F.Hroch (hroch@physics.muni.cz)
!
!  This file is part of Munipack.
!
!  Munipack 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.
!
!  Munipack 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 Munipack.  If not, see <http://www.gnu.org/licenses/>.
!
!
!  http://en.wikipedia.org/wiki/Backtracking
!
!  Key ideas of matching by backtracking has origin in book:
!    N.Wirth: Data Structures + Algorithms = Programs
!

module matcher

  implicit none

  integer, parameter, private :: rp = selected_real_kind(15)
  integer, private, dimension(:), allocatable :: id1, id2, seq1, seq2
  logical, private, dimension(:), allocatable :: idx1, idx2
  real(rp), private, dimension(:), pointer :: x1,y1,x2,y2,f1,f2
  real, private, dimension(:,:,:,:), allocatable :: tris1,tris2
  real(rp), private :: amin, dmin, fmin, tflux, sflux, scale
  real(rp), private :: sig1 = 1.0/3600.0 ! spherical coordinate limit [deg]
  real(rp), private :: sig2 = 1.0        ! on chip coordinate limit [pix]
  real(rp), private :: xsig = 2.0     ! Normal distribution limit at 95%
  real(rp), private :: rsig = 3.8     ! Rayleigh distribution limit at 95%
  real(rp), private :: fuse = 1       ! =1 .. use flux, =0.. don't use flux
  real(rp), private :: fsig = 1       ! flux limit in sigmas
  real(rp), private :: dtol1, dtol2, rtol1, rtol2
  integer, private :: nseq, nid
  integer, private :: minmatch = 5
  integer, private :: maxmatch = 30
  logical, private :: stopping = .false.
  logical, private :: progres2 = .false.

  integer, private :: counter ! debuging purposes

  private :: disp, angels, angles, escale, distances, seq, in_idx, qchi2, &
       elen, dsq, fsq, mflux

contains

subroutine match(x_1,y_1,f_1,x_2,y_2,f_2,nm,mm,sigma1,sigma2,fsigma, &
     fluxtest,id_1,id_2,fullmatch,luckymatch,matchprint,progress2,status)

  implicit none

  interface
     subroutine matchprint(id1,id2,c,t,p,d,r,q)
       integer, parameter :: dbl = selected_real_kind(15)
       integer, dimension(:), intent(in) :: id1,id2
       real(dbl), intent(in) :: c,t,p,d,r,q
     end subroutine matchprint

     subroutine progress2(ns,ntot,amin,dmin,fmin,id1,id2)
       integer, parameter :: dbl = selected_real_kind(15)
       integer, intent(in) :: ns,ntot
       integer, dimension(:), intent(in), optional :: id1,id2
       real(dbl), intent(in), optional :: amin,dmin,fmin
     end subroutine progress2

  end interface

  real(rp), dimension(:), intent(in), target :: x_1,y_1,x_2,y_2,f_1,f_2
  integer, dimension(:), allocatable, intent(out) :: id_1, id_2
  logical, intent(in) :: fullmatch, fluxtest
  integer, intent(in) :: nm, mm, luckymatch
  real(rp), intent(in) :: sigma1, sigma2, fsigma
  logical, intent(out) :: status
  integer :: i1,i2,j1,j2,ns,ntot
  real(rp) :: d1,d2,xlim

  status = .false.

  x1 => x_1
  y1 => y_1
  x2 => x_2
  y2 => y_2
  f1 => f_1
  f2 => f_2

  if( size(x1) /= size(y1) .or. size(x2) /= size(y2) ) &
       stop 'Match: Array dimensions does not matches.'

  if( (.not. all(f1 > 0)) .or. (.not. all(f2 > 0)) ) &
       stop 'Match: All fluxes must be > 0.'

  if( nm > 0 ) minmatch = nm
  if( mm > 0 ) maxmatch = mm
  if( sigma1 > 0.0 ) sig1 = sigma1
  if( sigma2 > 0.0 ) sig2 = sigma2
  if( fsigma > 0 ) fsig = fsigma

  if( .not. fluxtest ) fuse = 0

  allocate(id1(maxmatch),id2(maxmatch),seq1(0),seq2(0), &
       idx1(maxmatch),idx2(maxmatch), &
       tris1(maxmatch,maxmatch,maxmatch,3),tris2(maxmatch,maxmatch,maxmatch,3))

  tris1 = -1
  tris2 = -1

  nseq = 0
  amin = huge(amin)
  dmin = huge(dmin)
  fmin = huge(fmin)

  ns = 0
  ntot = (size(x1) - 1)*(size(x1)/2)

  dtol2 = xsig*sig2
  dtol1 = xsig*sig1
  rtol1 = rsig*sig1
  rtol2 = rsig*sig2
  xlim = qchi2(2)

  counter = 0

  do i1 = 1, size(x1) - 1
     do i2 = i1 + 1, size(x1)

        id1(1:2) = [ i1, i2 ]
        ns = ns + 1
        call progress2(ns,ntot)

        d1 = elen(x1(i1),y1(i1),x1(i2),y1(i2))
        if( .not. in_idx(seq1,id1(1:2)) .and. d1 > dtol1 ) then

           idx1 = .true.
           idx1(id1(1:2)) = .false.

           do j1 = 1, size(x2) - 1
              do j2 = j1+1, size(x2)

                 id2(1:2) = [ j1, j2 ]

                 nid = 2
                 d2 = elen(x2(j1),y2(j1),x2(j2),y2(j2))
                 idx2 = .true.
                 idx2(id2(1:2)) = .false.

                 ! Before launching of a long-duration search,
                 ! we step over the stars already presented
                 ! in previously formed sequences and those which
                 ! distances are under coordinate uncertainities.

                 if( .not. in_idx(seq2,id2(1:2)) .and. d2 > dtol2 ) then

                    ! check fluxes, optionaly
                    if( fuse > 0 ) then
                       call mflux(f1(id1(1:nid)),f2(id2(1:nid)),tflux,sflux)
                    else
                       tflux = 1
                       sflux = 0
                    end if

                    if( fuse*sflux / tflux < fsig*xlim ) then

                       ! setup of coordinate scale and the mean flux ratio
                       scale = escale(x1,y1,id1(1:nid),x2,y2,id2(1:nid))

                       ! start of recursive search
                       stopping = .false.
                       progres2 = .false.
                       call seq(matchprint)

                       ! show the longest sequence
                       if( progres2 ) &
                            call progress2(ns,ntot,amin,dmin,fmin,seq1,seq2)

                       ! finish on first success attempt (luckymatch == 0),
                       ! or finish when results looks reliable (>0)
                       if(  .not. fullmatch &
                            .and. nseq >= minmatch + luckymatch ) goto 666

                    end if
                 end if
              end do
           end do
        end if
     end do
  end do

666 continue

!  write(*,*) counter

  if( nseq > 0 ) then

     allocate(id_1(nseq),id_2(nseq))
     id_1 = seq1
     id_2 = seq2
     status = .true.

  end if

  deallocate(id1,id2,idx1,idx2,seq1,seq2,tris1,tris2)

end subroutine match


! the core
recursive subroutine seq(matchprint)

  interface
     subroutine matchprint(id1,id2,c,t,a,d,r,q)
       integer, parameter :: dbl = selected_real_kind(15)
       integer, dimension(:), intent(in) :: id1,id2
       real(dbl), intent(in) :: c,t,a,d,r,q
     end subroutine matchprint
  end interface

  real(rp), parameter :: sq2 = sqrt(2.0)

  integer :: i,j,i1,i2,j1,j2,i0,j0,n1
  real(rp) :: u1, v1, u2, v2, d1, d2, tol, &
       tol1, tol2, au, av, fres, sres, a, d, f, t, s, xlim, xlin
  logical :: launch

  if( stopping ) return

  n1 = nid - 1
  i1 = id1(n1)
  i2 = id1(nid)
  j1 = id2(n1)
  j2 = id2(nid)

  xlin = qchi2(nid)
  xlim = qchi2(2)

  launch = .false.
  do i = 1, size(x1)

     if( idx1(i) ) then

        if( tris1(i1,i2,i,1) < 0.0 ) then

           call triangle(x1(i1),y1(i1),x1(i2),y1(i2),x1(i),y1(i),d1,u1,v1)
           tris1(i1,i2,i,:) = real([u1,v1,d1])
!           counter = counter + 1

        else
           u1 = tris1(i1,i2,i,1)
           v1 = tris1(i1,i2,i,2)
           d1 = tris1(i1,i2,i,3)
        end if

        tol1 = sq2 * rtol1 / d1

        ! v1 > (1 + tol1 - u1) rejects triangles with too short side
        if( d1 > dtol1 .and. v1 > (1 + tol1 - u1) ) then

           ! counter = counter + 1

           do j = 1, size(x2)

              if( fuse > 0 ) then
                 fres = fsq(tflux,sflux,[f1(i)],[f2(j)])
              else
                 fres = 0
              end if

              if( idx2(j) .and. fres*fuse < fsig*xlim ) then

                 if( tris2(j1,j2,j,1) < 0.0 ) then

                    call triangle(x2(j1),y2(j1),x2(j2),y2(j2),x2(j),y2(j), &
                         d2,u2,v2)
                    tris2(j1,j2,j,:) = real([u2,v2,d2])

                 else
                    u2 = tris2(j1,j2,j,1)
                    v2 = tris2(j1,j2,j,2)
                    d2 = tris2(j1,j2,j,3)
                 end if

                 tol2 = sq2 * rtol2 / d2

                 if( d2 > dtol2 .and. v2 > (1 + tol2 - u2)  ) then

                    tol = tol1 + tol2

                    au = abs(u1 - u2)
                    av = abs(v1 - v2)
                    sres = dsq(scale,[d1],[d2],sig1,sig2)

                    if( au < tol .and. av < tol .and. sres < xlim ) then

                       nid = nid + 1
                       id1(nid) = i
                       id2(nid) = j
                       idx1(i) = .false.
                       idx2(j) = .false.

                       if( .not. stopping ) then
                          launch = .true.
                          call seq(matchprint)
                       end if

                       if( nid >= max(minmatch,nseq) ) then

                          ! final checks for realiable match:

                          ! dispersions of distances and angles
                          scale = escale(x1,y1,id1(1:nid),x2,y2,id2(1:nid))
                          d = disp(scale,x1,y1,id1(1:nid),x2,y2,id2(1:nid))
                          a = angels(x1,y1,id1(1:nid),x2,y2,id2(1:nid))

                          ! dispersion of fluxes (optional)
                          if( fuse > 0 ) then
                             call mflux(f1(id1(1:nid)),f2(id2(1:nid)),t,s)
                             f = fsq(t,s,f1(id1(1:nid)),f2(id2(1:nid)))
                          else
                             f = 0
                          end if

                          ! head and tail of sequence on similarity of triangles
                          i0 = id1(1)
                          j0 = id2(1)
                          call triangle(x1(i0),y1(i0),x1(i2),y1(i2), &
                               x1(i),y1(i),d1,u1,v1)
                          call triangle(x2(j0),y2(j0),x2(j2),y2(j2), &
                               x2(j),y2(j),d2,u2,v2)
                          tol = sq2*(rtol1/d1 + rtol2/d2)

                          if( a < xlin &
                               .and. d < xlin &
                               .and. f*fuse < fsig*xlin &
                               .and. abs(u1 - u2) < tol &
                               .and. abs(v1 - v2) < tol ) then

                             progres2 = .true.
                             call matchprint(id1(1:nid),id2(1:nid),scale,tflux,&
                                  a,d,f,xlin)
                             if( nid >= nseq ) then
                                nseq = nid
                                amin = a
                                dmin = d
                                fmin = f
                                deallocate(seq1,seq2)
                                allocate(seq1(nseq),seq2(nseq))
                                seq1 = id1(1:nid)
                                seq2 = id2(1:nid)
                             end if
                          end if
                       end if

                       nid = nid - 1
                       idx1(i) = .true.
                       idx2(j) = .true.

                    end if
                 end if
              end if
           end do ! j
        end if
     end if
  end do ! i

  ! none matching stars has been found,
  ! =>  next subroutine invocation will be stopped
  if( .not. launch ) stopping = .true.

end subroutine seq


function elen(x1,y1,x2,y2)

  ! lenght of an edge by Pythagorean theorem

  real(rp) :: elen
  real(rp), intent(in) :: x1,y1,x2,y2

  elen = sqrt((x1 - x2)**2 + (y1 - y2)**2)

end function elen


subroutine triangle(x1,y1,x2,y2,x3,y3,d3,u,v)

  ! computes coordinates in uv space

  real(rp), parameter :: eps = epsilon(0.0_rp)

  real(rp), intent(in) :: x1,y1,x2,y2,x3,y3
  real(rp), intent(out) :: d3,u,v

  real(rp), dimension(0:3) :: d
  real(rp) :: x
  integer :: i,j

  ! compute lengths of triangle's sides
  d(1) = sqrt((x1 - x2)**2 + (y1 - y2)**2)
  d(2) = sqrt((x1 - x3)**2 + (y1 - y3)**2)
  d(3) = sqrt((x2 - x3)**2 + (y2 - y3)**2)

  ! sorting, an implementation of the insert sort algorithm by N.Wirth
  do i = 2, 3
     x = d(i)
     d(0) = x
     j = i - 1
     do while ( x < d(j) )
        d(j+1) = d(j)
        j = j - 1
     end do
     d(j+1) = x
  end do

  if( d(3) > eps ) then
     ! compute triangle coordinates
     u = d(1)/d(3)
     v = d(2)/d(3)
     ! the longest side
     d3 = d(3)
  else
     ! triangle is collapsing to a point
     u = 0.0
     v = 0.0
     d3 = 0.0
  end if

end subroutine triangle



subroutine distances(x,y,id,d)

  real(rp), dimension(:), intent(in) :: x,y
  integer, dimension(:), intent(in) :: id
  real(rp), dimension(:), intent(out) :: d
  integer :: i,j,k,l,n

  n = size(id)
  l = n  ! connecting head and tail
  do k = 1, n
     i = id(k)
     j = id(l)
     d(k) = elen(x(i),y(i),x(j),y(j))
     l = k
  end do

end subroutine distances

function escale(x1,y1,id1,x2,y2,id2)

  ! estimate scale from ratio of edge lenghts

  real(rp) :: escale
  integer, dimension(:), intent(in) :: id1, id2
  real(rp), dimension(:), intent(in) :: x1,y1,x2,y2
  real(rp), dimension(size(id1)) :: d1,d2

  call distances(x1,y1,id1,d1)
  call distances(x2,y2,id2,d2)

  escale  = sum(d2/d1)/size(id1)

end function escale


function dsq(s,d1,d2,sig1,sig2)

  real(rp) :: dsq
  real(rp), parameter :: sq2 = sqrt(2.0_rp)
  real(rp), intent(in) :: s
  real(rp), dimension(:), intent(in) :: d1,d2
  real(rp), intent(in) :: sig1,sig2
  real(rp), dimension(size(d1)) :: r2,dr2

  dr2 = (s*sig1)**2 + sig2**2
  where( dr2 > 0 )
     r2 = (d2 - s*d1)**2 / (2*dr2)
  elsewhere
     r2 = 1
  end where
  dsq = sum(r2)

end function dsq

function disp(s,x1,y1,id1,x2,y2,id2)

  ! Approximated standard residuals of edge lenghts
  ! on frame and catalogue in pixels (lenghts in degrees
  ! are scaled):
  !            (d2 - s*d1)**2
  !            --------------
  !                sig**2
  ! where sig = 1.41*sqrt((s*sig1)**2 + sig2**2).

  real(rp) :: disp
  real(rp), intent(in) :: s
  integer, dimension(:), intent(in) :: id1, id2
  real(rp), dimension(:), intent(in) :: x1,y1,x2,y2
  real(rp), dimension(size(id1)) :: d1,d2

  call distances(x1,y1,id1,d1)
  call distances(x2,y2,id2,d2)

  disp = dsq(s,d1,d2,sig1,sig2)

end function disp


subroutine angles(x,y,id,sig,a,da)

  ! computes angles in polygon given by the sequence
  ! the angles and theirs errors are in radians

  real(rp), dimension(:), intent(in) :: x,y
  integer, dimension(:), intent(in) :: id
  real(rp), intent(in) :: sig
  real(rp), dimension(:), intent(out) :: a,da
  real(rp), dimension(2) :: u,v
  real(rp) :: uu,vv,t
  integer :: i,l,k,n

  n = size(id)

  l = n
  do i = 1, n
     k = mod(i,n) + 1
     u = [ x(id(i)) - x(id(l)), y(id(i)) - y(id(l)) ]
     v = [ x(id(k)) - x(id(i)), y(id(k)) - y(id(i)) ]
     uu = sqrt(sum(u**2))
     vv = sqrt(sum(v**2))
     t = sum(u*v)/(uu*vv)
     a(i) = acos(t)
     da(i) = t*sig*sqrt(1/uu**2 + 1/vv**2)
     l = i
  end do

end subroutine angles


function angels(x1,y1,id1,x2,y2,id2)

  ! returns sum of squares suitable for test by Xi2 distribution
  ! of residuals of angles:
  !
  !            |a1 - a2|
  !            ---------
  !              sig
  !
  ! where sig=sqrt(da1**2 + da2**2)

  real(rp) :: angels
  integer, dimension(:), intent(in) :: id1, id2
  real(rp), dimension(:), intent(in) :: x1,y1,x2,y2
  real(rp), dimension(size(id1)) :: a1,a2,da1,da2,r

  call angles(x1,y1,id1,sig1,a1,da1)
  call angles(x2,y2,id2,sig2,a2,da2)

  r = (a1 - a2)/sqrt(da1**2 + da2**2)
  angels = sum(r**2)

end function angels


subroutine mflux(f1,f2,t,s)

  ! determines estimation of ratio and its standard deviation
  ! by arithmetical mean (intentionally non-robust)

  real(rp), dimension(:), intent(in) :: f1,f2
  real(rp), intent(out) :: t,s

  real(rp), dimension(size(f1)) :: r
  integer :: n

  n = size(r)
  r = f1 / f2
  t = sum(r)/n
  s = sqrt(sum((r-t)**2)/(n-1))

  ! s == 0 for twices with the same magnitudes (very low probability)
  if( s < 10*epsilon(s) ) then
     s = (sum(sqrt(f1)/f1) / n + sum(sqrt(f2)/f2) / n) / 2
  end if

end subroutine mflux


function fsq(t,s,f1,f2)

  ! returns squares of residuals of
  !   (f1 - t* f2)**2 / sqrt(f1 + t**2*f2 + f2**s**2)**2
  !  or simplified form
  !   (f1/f2 - t)**2 / s**2
  ! suitable for test by chi2 statistics
  !
  ! the last term of errors describes t noise and
  ! generaly is due effects not directly connected
  ! on photon statistics (clouds, background, ..)
  ! Fluxes (normal distribution) are considered.

  real(rp) :: fsq
  real(rp),intent(in) :: t,s
  real(rp), dimension(:), intent(in) :: f1,f2

  fsq = sum((f1/f2 - t)**2 / s**2)

end function fsq


function in_idx(seq,id)

  logical :: in_idx
  integer, dimension(:), intent(in) :: seq,id
  logical, dimension(size(id)) :: n
  integer :: i,j,l

  n = .false.
  l = 1
  do j = 1,size(id)
     do i = l,size(seq)
        if( seq(i) == id(j) ) then
           n(j) = .true.
           l = i
        end if
     end do
  end do
  in_idx = all(n)

end function in_idx

function qchi2(n)

  ! approximation of quantiles of chi2 distribution for choosed probability
  ! accuracy for n < 30 is poor (few thents only)
  ! u are quantiles of N(0,1):
  !    P     u
  !   0.9   1.282
  !   0.95  1.645
  !   0.99  2.326
  !   0.999 3.090
  !

  real :: qchi2
  real, parameter :: u = 1.645
  integer, intent(in) :: n

  qchi2 = (sqrt(2.0*n - 1.0) + u)**2 / 2.0

end function qchi2

end module matcher
