!
! absfit - fitting absolute deviations
!
!
! Copyright © 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/>.
!

module estimator

  use iso_fortran_env

  implicit none

  logical, private :: debug = .true.
  integer, parameter, private :: dbl = selected_real_kind(15)
  real(dbl), parameter, private :: rad = 57.295779513082322865_dbl

  private :: escaler,erot,angle,ereflex,eoff

contains

  subroutine inestim(type,a,d,xc,yc,xx,yy,acen,dcen,sc,dsc,rot,drot,refl, &
       xoff,dxoff,yoff,dyoff,verbose)

    use astrotrafo

    character(len=*),intent(in) :: type
    real(dbl),intent(in) :: xc,yc
    real(dbl),dimension(:),intent(in) :: a,d,xx,yy
    real(dbl),intent(inout) :: acen,dcen,sc,rot,dsc,drot,xoff,yoff,dxoff,dyoff
    real(dbl),intent(out) :: refl
    logical, intent(in) :: verbose
    real(dbl), dimension(:), allocatable :: x,y,u,v
    integer :: ndat
    type(AstroTrafoProj) :: t

    if( size(a) < 1 ) stop 'InEstim: no points available.'

    if(verbose) write(error_unit,*) "=== Parameters estimation ==="

    debug = verbose

    ndat = size(a)
    allocate(x(ndat),y(ndat),u(ndat),v(ndat))

    ! projection of spherical coordinates to a tangent plane
    call trafo_init(t,type,acen,dcen)
    call proj(t,a,d,u,v)

    call escaler(u,v,xx,yy,sc,dsc)
    call ereflex(u,v,xx,yy,refl)

    ! time to estimate an angle of rotation
    call trafo_init(t,xcen=xc,ycen=yc,scale=sc,refl=refl)
    call invaffine(t,xx,yy,x,y)
    call erot(u,v,x,y,rot,drot)

    ! offset
    ! origin of rectangular coordinates is moved to center of frame
    ! because that's point the frame is rotated around
    call trafo_init(t,xcen=xc,ycen=xc,scale=sc,rot=rot,refl=refl)
    call invaffine(t,xx,yy,x,y)
    call eoff(u,v,x,y,xoff,dxoff,yoff,dyoff)

  end subroutine inestim


  subroutine escaler(u,v,x,y,c,dc)

    use oakleaf

    real(dbl), dimension(:),intent(in) :: x,y,u,v
    real(dbl), intent(out) :: c,dc

    integer :: i,j,n
    real(dbl),dimension(:),allocatable :: cc
    real(dbl) :: d1,d2,e

    allocate(cc(size(x)**2))

    n = 0
    do i = 2, size(x)
       do j = 1,i-1
          d1 = sqrt((x(j) - x(i))**2 + (y(j) - y(i))**2)
          d2 = sqrt((u(j) - u(i))**2 + (v(j) - v(i))**2)
          n = n + 1
          cc(n) = d2/d1
!         if(debug) write(*,'(a,2es15.8,3f15.5)') '#',x(i),y(i),u(i),v(i),d2/d1
       end do
    enddo

    if( n > 1 ) then
       call rmean(cc(1:n),c,e,dc)
    else
       c = 0.0_dbl
       dc = 0.0_dbl
    end if

    if( debug ) write(error_unit,'(a,f10.1,g10.2)') &
         '# estim init scale [pix/deg]: ',1/c,dc/c**2

  end subroutine escaler

  subroutine erot(u,v,x,y,f,df)

    use oakleaf

    real(dbl), dimension(:),intent(in) :: x,y,u,v
    real(dbl), intent(out) :: f,df

    integer :: i,j,n
    real(dbl),dimension(:),allocatable :: cc,cs
    real(dbl) :: d1,d2,x1,y1,x2,y2,c,s,dc,ds,e

    n = size(x) - 1
    allocate(cc(n),cs(n))

    do i = 1, n
       j = i + 1
       x1 = x(j) - x(i)
       y1 = y(j) - y(i)
       d1 = sqrt(x1**2 + y1**2)
       x2 = u(j) - u(i)
       y2 = v(j) - v(i)
       d2 = sqrt(x2**2 + y2**2)
       ! cos
       cc(i) = (x1*x2 + y1*y2)/(d1*d2)
       ! sin
       cs(i) = (x1*y2 - x2*y1)/(d1*d2)
!       if(debug)write(*,'(a,6f12.5)') '# erot ',x(i),y(i),u(i),v(i),cc(i),cs(i)
    enddo

    if( n > 1 ) then
       call rmean(cc,c,e,dc)
       call rmean(cs,s,e,ds)
       f = rad * sign(atan2(abs(s),c),s)
       if( abs(s) > 0 ) then
          df = rad*sqrt(ds**2 + (c/s)**2*dc**2)
       else
          df = max(ds,dc)
       end if
    else
       f = 0.0_dbl
       df = 0.0_dbl
    end if

    if( debug ) write(error_unit,'(a,f8.3,g10.2)') &
         '# estim init angle [deg]: ',f,df

  end subroutine erot


  function angle(x1,y1,x2,y2,x3,y3)

    real(dbl), intent(in) :: x1,y1,x2,y2,x3,y3
    real(dbl) :: angle,u1,u2,v1,v2,c,s,r1,r2

    u1 = x1 - x2
    u2 = x3 - x2
    v1 = y1 - y2
    v2 = y3 - y2
    r1 = sqrt(u1**2 + v1**2)
    r2 = sqrt(u2**2 + v2**2)
    c = (u1*u2 + v1*v2)/(r1*r2)
    s = (u1*v2 - u2*v1)/(r1*r2)
    angle = sign(atan2(abs(s),c),s)

  end function angle


  subroutine ereflex(u,v,x,y,refl)

    use oakleaf

    real(dbl), dimension(:),intent(in) :: x,y,u,v
    real(dbl), intent(out) :: refl

    integer :: i,j,k
    integer, dimension(:), allocatable :: r
    real(dbl) :: f1,f2

    allocate(r(size(x)-2))

    refl = 1.0_dbl
    do i = 1, size(x)-2
       j = i + 1
       k = i + 2
       f1 = angle(x(i),y(i),x(j),y(j),x(k),y(k))
       f2 = angle(u(i),v(i),u(j),v(j),u(k),v(k))

       if( f1*f2 >= 0 ) then
          r(i) = 1
       else
          r(i) = -1
       end if
!       if( debug ) write(*,'(a,3f7.2)') '# ereflex ',f1,f2,f1/f2
    enddo

    if( count(r > 0) > count(r < 0) ) then
       refl = 1.0_dbl
    else
       refl = -1.0_dbl
    end if

    if( debug ) write(error_unit,'(a,f5.0)') '# estim init reflexion: ',refl

  end subroutine ereflex

  subroutine eoff(u,v,x,y,xoff,dxoff,yoff,dyoff)

    use oakleaf

    real(dbl), dimension(:), intent(in) :: x,y,u,v
    real(dbl), intent(out) :: xoff,dxoff,yoff,dyoff

    integer :: i,n
    real(dbl),dimension(:),allocatable :: du,dv
    real(dbl) :: e

    n = size(x)
    allocate(du(n),dv(n))

    do i = 1, n
       du(i) = u(i) - x(i)
       dv(i) = v(i) - y(i)
!       if( debug ) write(*,'(6f11.5)') x(i),y(i),u(i),v(i),du(i),dv(i)
    end do

    if( n > 1 ) then
       call rmean(du,xoff,e,dxoff)
       call rmean(dv,yoff,e,dyoff)
    else
       xoff = 0.0_dbl
       yoff = 0.0_dbl
       dxoff = 0.0_dbl
       dyoff = 0.0_dbl
    end if

    if( debug ) write(error_unit,'(a,2f12.5,2g10.2)') &
         '# estim init offset [deg]: ',xoff,yoff,dxoff,dyoff

  end subroutine eoff

end module estimator
