#
#  gpsman --- GPS Manager: a manager for GPS receiver data
#
#  Copyright (c) 2004 Miguel Filgueiras (mig@ncc.up.pt) / Universidade do Porto
#
#    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 2 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.
#
#  File: maptransf.tcl
#  Last change:  13 July 2004
#

## Uses information kindly supplied by
#     Jose Alberto Goncalves, Universidade do Porto
##

## transformations for geo-referencing of images
#
# (xt,yt) terrain coordinates (metre)
# (xm,-ym) map coordinates (pixel, ym grows downwards!)
#
# affine transformation, 6 parameters
#  (xt,yt) = [ aij ] x (xm,ym) + (e,f)
#
# affine conformal transformation, 4 parameters
#  (xt,yt) = lambda [ aij ] x (xm,ym) + (e,f)
#   where  a11 = cos a   a12 = -sin a   a21 = sin a   a22 = cos a
#     rotation angle: a    scaling factor: lambda
#
# affine conformal no rotation transformation, 3 parameters
#  (xt,yt) = lambda (xm,ym) + (e,f)
##

## when adding new transformation procedures the variables MAPTRANSFNPTS and
#   MAPKNOWNTRANSFS must be changed in gpsman.tcl
# see also relevant procedures in map.tcl, command_parse.tcl, command.tcl

     # indices of MTData array used for each transformation
array set MAPTRANSFDATA {
    NoRot      {lambda e f}
    Affine     {a b c d e f det k2m1 k4m3}
    AffineConf {a b e f det k2m1 k4m3}
}

proc MapTransfIs {transf} {
    # set global variables so that map transformation is $transf
    #  $transf in $MAPKNOWNTRANSFS
    global MapTransf MapTransfTitle TXT

    set MapTransf $transf ; set MapTransfTitle $TXT(TRNSF$transf)
    return
}

## transformation procs

proc MapInitNoRotTransf {scale xt0 yt0 xm0 ym0} {
    # compute parameters of affine conformal transformation with no rotation
    #  $scale is in metre/pixel
    #  $xt0,$yt0: terrain coordinates of the map point at $xm0,$ym0 pixel
    global MTData

    MapTransfIs NoRot
    # to avoid integer divisions
    set MTData(lambda) [expr $scale*1.0]
    set MTData(e) [expr $xt0-$scale*$xm0]
    set MTData(f) [expr $yt0+$scale*$ym0]
    return 0
}

proc InitNoRotTransf {} {
    # compute representation of affine conformal transformation with no
    #  rotation transformation from 2 WPs
    #  $MapLoadWPs is list of indices of relevant WPs
    #  $MapLoadPos($n,x), $MapLoadPos($n,y)  $n in 0..2 give pixel coordinates
    # set MapScale to appropriate value in metre/pixel
    # return 0 if failed to solve equations
    global MapLoadPos MapScale MTData

    MapTransfIs NoRot
    foreach p "0 1" tcs [MapGeoRefPoints 2] {
	foreach "xt$p yt$p" $tcs {}
    }
    # distance between points in the terrain (metre)
    set dxt [expr $xt0-$xt1] ; set dyt [expr $yt0-$yt1]
    # must have both multiplications by 1.0 !!!
    set lt [expr sqrt(1.0*$dxt*$dxt+1.0*$dyt*$dyt)]
    # distance between points in the map (pixel)
    set dxm [expr $MapLoadPos(0,x)-$MapLoadPos(1,x)]
    set dym [expr $MapLoadPos(0,y)-$MapLoadPos(1,y)]
    # must have both multiplications by 1.0 !!!
    set lm [expr sqrt(1.0*$dxm*$dxm+1.0*$dym*$dym)]
    # scale
    if { [catch {set scale [expr 1.0*$lt/$lm]}] } { return 0 }
    set MapScale $scale
    set MTData(lambda) [expr $scale*1.0]
    set MTData(e) [expr $xt0-$scale*$MapLoadPos(0,x)]
    set MTData(f) [expr $yt0+$scale*$MapLoadPos(0,y)]
    return 1
}

proc MapApplyNoRotTransf {xt yt} {
    # apply affine conformal transformation with no rotation
    global MTData

    set s $MTData(lambda)
    return [list [expr ($xt-$MTData(e))/$s] [expr ($MTData(f)-$yt)/$s]]
}

proc MapInvertNoRotTransf {xm ym} {
    # invert affine conformal transformation with no rotation
    global MTData

    set s $MTData(lambda)
    return [list [expr $s*$xm+$MTData(e)] [expr $MTData(f)-$s*$ym]]
}

proc MapNewScaleNoRotTransf {scale} {
    # set transformation parameters after change in map scale
    # return 1 if possible
    global MTData

    set MTData(lambda) [expr $scale*1.0]
    return 1
}

proc MapInitAffineTransf {args} {
    # compute representation of affine transformation from 3 points
    #  $args==""   points are WPs, with $MapLoadWPs the list of
    #          indices of relevant WPs, used by proc MapGeoRefPoints
    #          that sets MapLoadPos(_,_) to the pixel coordinates
    #       ==list of projected coordinates, MapLoadPos(_,_) was already set
    #  $MapLoadPos($n,x), $MapLoadPos($n,y)  $n in 0..2 give pixel coordinates
    # set MapScale to appropriate value in metre/pixel
    # return 0 if failed to solve equations
    global MapLoadPos MapScale MTData Mat Rx

    MapTransfIs Affine
    if { $args == "" } {
	if { [set tcs [MapGeoRefPoints 3]] == -1 } { return 0 }
    } else { set tcs [lindex $args 0] }
    foreach ps [list "a b e" "c d f"] d "0 1" {
	foreach i "0 1 2" {
	    set Mat($i,0) $MapLoadPos($i,x)
	    set Mat($i,1) $MapLoadPos($i,y)
	    set Mat($i,2) 1
	    set Mat($i,3) [lindex [lindex $tcs $i] $d]
	}
	if { [GaussReduce 3] != 1 } { return 0 }
	foreach p $ps i "0 1 2" {
	    set MTData($p) $Mat($Rx($i),3)
	}
    }
    # the following parameters simplify computations
    set MTData(det) [expr $MTData(a)*$MTData(d)-$MTData(b)*$MTData(c)]
    set MTData(k2m1) [expr $MTData(b)*$MTData(f)-$MTData(d)*$MTData(e)]
    set MTData(k4m3) [expr $MTData(c)*$MTData(e)-$MTData(a)*$MTData(f)]
    # scale along the xm-axis when variation of ym=0
    set MapScale [expr abs($MTData(a))]
    return 1
}

proc MapApplyAffineTransf {xt yt} {
    # apply affine transformation
    global MTData

    set x [expr ($MTData(d)*$xt-$MTData(b)*$yt+$MTData(k2m1))/$MTData(det)]
    set y [expr ($MTData(a)*$yt-$MTData(c)*$xt+$MTData(k4m3))/$MTData(det)]
    return [list $x $y]
}

proc MapInvertAffineTransf {xm ym} {
    # invert affine transformation
    global MTData

    set xt [expr $MTData(a)*$xm+$MTData(b)*$ym+$MTData(e)]
    set yt [expr $MTData(c)*$xm+$MTData(d)*$ym+$MTData(f)]
    return [list $xt $yt]
}

proc MapNewScaleAffineTransf {scale} {
    # set transformation parameters after change in map scale
    # return 1 if possible

    return 0
}

proc MapInitAffineConfTransf {args} {
    # compute representation of affine conformal transformation from 2 points
    #  $args==""   points are WPs, with $MapLoadWPs the list of
    #          indices of relevant WPs, used by proc MapGeoRefPoints
    #          that sets MapLoadPos(_,_) to the pixel coordinates
    #       ==list of projected coordinates, MapLoadPos(_,_) was already set
    #  $MapLoadPos($n,x), $MapLoadPos($n,y)  $n in 0..1 give pixel coordinates
    # set MapScale to appropriate value in metre/pixel
    # return 0 if failed to solve equations
    global MapLoadPos MapScale MTData Mat Rx

    MapTransfIs AffineConf
    catch {unset MTData}
    if { $args == "" } {
	if { [set tcs [MapGeoRefPoints 2]] == -1 } { return 0 }
    } else { set tcs [lindex $args 0] }
    foreach e "0 2" i "0 1" {
	set xyt [lindex $tcs $i]
	set Mat($e,0) $MapLoadPos($i,x)
	set Mat($e,1) [expr -$MapLoadPos($i,y)]
	set Mat($e,2) 1 ; set Mat($e,3) 0
	set Mat($e,4) [lindex $xyt 0]
	incr e
	set Mat($e,0) [expr -$MapLoadPos($i,y)]
	set Mat($e,1) [expr -$MapLoadPos($i,x)]
	set Mat($e,2) 0 ; set Mat($e,3) 1
	set Mat($e,4) [lindex $xyt 1]
    }
    if { [GaussReduce 4] != 1 } { return 0 }
    foreach p "a b e f" i "0 1 2 3" {
	set MTData($p) $Mat($Rx($i),4)
    }
    # the following parameters make calculations easier
    set MTData(det) [expr $MTData(a)*$MTData(a)+$MTData(b)*$MTData(b)]
    set MTData(k2m1) [expr $MTData(b)*$MTData(f)-$MTData(a)*$MTData(e)]
    set MTData(k4m3) [expr $MTData(b)*$MTData(e)+$MTData(a)*$MTData(f)]
    # scale along the xm-axis when variation of ym=0
    set MapScale [expr abs($MTData(a))]
    return 1
}

proc MapApplyAffineConfTransf {xt yt} {
    # apply affine conformal transformation
    global MTData

    set xm [expr ($MTData(a)*$xt-$MTData(b)*$yt+$MTData(k2m1))/$MTData(det)]
    set ym [expr -($MTData(a)*$yt+$MTData(b)*$xt-$MTData(k4m3))/$MTData(det)]
    return [list $xm $ym]
}

proc MapInvertAffineConfTransf {xm ym} {
    # invert affine conformal transformation
    global MTData

    set xt [expr $MTData(a)*$xm-$MTData(b)*$ym+$MTData(e)]
    set yt [expr -$MTData(b)*$xm-$MTData(a)*$ym+$MTData(f)]
    return [list $xt $yt]
}

proc MapNewScaleAffineConfTransf {scale} {
    # set transformation parameters after change in map scale
    # return 1 if possible

    return 0
}

## solving a linear system of equations nxn by Gauss-Jordan elimination
# code adopted from the Slopes Algorithm implementation in C
# by M Filgueiras and A P Tomas / Universidade do Porto / 1996, 1997
#

proc GaussNullFirst {k n} {
    # check first row with non-null element in $k,$k using Rx, Cx and
    #  exchange rows if needs be
    #  $n is dimension of matrix
    # return 0 if non-null element found
    global Rx Cx Mat

    for { set i [expr $k+1] } { $i < $n } { incr i } {
	if { $Mat($Rx($i),$Cx($k)) != 0 } {
	    set l $Rx($k) ; set Rx($k) $Rx($i) ; set Rx($i) $l
	    return 0
	}
    }
    return 1
}

proc GaussElim {i k n p} {
    # eliminate $i,$k element on ?x($n+1) matrix, pivot $p at $k,$k,
    #  using Rx, Cx; the $i,$k element is assumed to be non-null
    global Rx Cx Mat

    set ii $Rx($i) ; set ik $Rx($k)
    set m [expr 1.0*$Mat($ii,$Cx($k))/$p]
    for { set j [expr $k+1] } { $j <= $n } { incr j } {
	set jj $Cx($j)
	set Mat($ii,$jj) [expr $Mat($ii,$jj)-$m*$Mat($ik,$jj)]
    }
    set Mat($ii,$Cx($k)) 0
    return
}

proc GaussSubelim {k n} {
    # eliminate below $k on nx(n+1) matrix, using Rx, Cx[]
    global Rx Cx Mat

    set ck $Cx($k)
    set p $Mat($Rx($k),$ck)
    for { set i [expr $k+1] } { $i < $n } { incr i } {
	if { $Mat($Rx($i),$ck) != 0 } {
	    GaussElim $i $k $n $p
	}
    }
    return
}

proc GaussSupraelim {i n} {
    # eliminate above $i on _x($n1) matrix, using Rx, Cx
    global Rx Cx Mat

    set ci $Cx($i)
    set p $Mat($Rx($i),$ci)
    for { set a 0 } { $a < $i } { incr a } {
	if { $Mat($Rx($a),$ci) != 0 } {
	    GaussElim $a $i $n $p
	}
    }
    return
}

proc GaussReduce {n} {
    # reduction of $nx($n+1) matrix, using Rx, Cx to index rows and columns
    # indices start from 0
    # values in global array $Mat are changed by this procedure
    # return 1 if there are is only one solution
    # solutions to be retrived from $Mat($Rx($i),$n) for each $i from 0 to $n-1
    global Rx Cx Mat

    for { set i 0 } { $i < $n } { incr i } {
	set Rx($i) $i ; set Cx($i) $i
    }
    set Cx($n) $n
    for { set i 0 } { $i < $n } { incr i } {
	if { $Mat($Rx($i),$Cx($i))==0 && [GaussNullFirst $i $n] } { return 0 }
	GaussSubelim $i $n
    }
    for { set i [expr $n-1] } { $i > -1 } { incr i -1 } {
	set Mat($Rx($i),$n) [expr $Mat($Rx($i),$n)/$Mat($Rx($i),$Cx($i))]
	set Mat($Rx($i),$Cx($i)) 1
	GaussSupraelim $i $n
    }
    return 1
}

##### TFW metadata file
# there is no publicly available definition for this format:
#   each file has 6 lines with a number each referred to by tfw_ below
#
# assume this is an affine transformation with
# (xt,yt) = [ aij ] x (xm,ym) + (e,f)
#  where  a11 = tfw1*cos(tfw2)
#         a12 = -tfw4*sin(tfw3)
#         a21 = tfw1*sin(tfw2)
#         a22 = tfw4*cos(tfw3)
#         e = tfw5, f = tfw6
# 
# inverse computation (not used): from aij to tfw_   TO BE CHECKED!
#     if a11 != 0
#       tfw2 = atan2(a21,a11) ; tfw1 = c/cos(tfw2)
#     elseif tfw1 is known to be positive (W-E orientation from bottom to top)
#       if a21 > 0
#         tfw2 = PI/2 ; tfw1 = a21
#       else  tfw2 = 1.5*PI ; tfw1 = -a21
#     elseif a21 > 0
#       tfw2 = 1.5*PI ; tfw1 = -a21
#     else  tfw2 = PI/2 ; tfw1 = a21
#
#  and similarly for a12, a22, tfw3, tfw4

proc MapInitTFWTransf {data proj} {
    # compute parameters of affine transformation from the values in a
    #  TFW metadata file
    # assume this is an affine transformation with
    # (xt,yt) = [ aij ] x (xm,ym) + (e,f)
    #  where  a11 = tfw1*cos(tfw2)
    #         a12 = -tfw4*sin(tfw3)
    #         a21 = tfw1*sin(tfw2)
    #         a22 = tfw4*cos(tfw3)
    #         e = tfw5, f = tfw6
    #  $data is a pair with an empty list and a list with the 6 parameters
    #        (tfw_ in the formulae above)
    #  $proj is the projection
    # return 0 if determinant is 0
    global MTData MapScale

    MapTransfIs Affine
    foreach "t1 t2 t3 t4 MTData(e) MTData(f)" [lindex $data 1] {}
    array set MTData [list \
	    a [expr $t1*cos($t2)]  b [expr -$t4*sin($t3)] \
	    c [expr $t1*sin($t2)]  d [expr $t4*cos($t3)]  ]
    set MTData(det) [expr $MTData(a)*$MTData(d)-$MTData(b)*$MTData(c)]
    if { abs($MTData(det)) < 1e-12 } { return 0 }	    
    set MTData(k2m1) [expr $MTData(b)*$MTData(f)-$MTData(d)*$MTData(e)]
    set MTData(k4m3) [expr $MTData(c)*$MTData(e)-$MTData(a)*$MTData(f)]
    FixMapScale $proj
    return 1
}

##### partial support for OziExplorer .map files

proc MapInitOziMapTransf {data proj} {
    # use datum and geodetic and pixel coordinates of points in Ozi .map
    #  file to compute parameters of affine or affine conformal transformation
    #  $data is pair with list of latd,longd,datum and
    #        list of pairs with pixel coordinates (in Tcl sign convention)
    #        aligned with the previous one
    #  $proj is the projection
    # return 0 on error
    global MTData MapScale MPData MapLoadPos

    foreach "opds opxs" $data {}
    if { [set sel [MapReduceCPoints $opxs]] == "" } { return 0 }
    set n 0
    set projcs ""
    foreach ix $sel {
	foreach "x y" [lindex $opxs $ix] {}
	set MapLoadPos($n,x) $x ; set MapLoadPos($n,y) $y
	set lld [lindex $opds $ix]
	lappend projcs [eval Proj${proj}Point MPData $lld]
	incr n
    }
    if { $n == 3 } {
	set r [MapInitAffineTransf $projcs]
    } else { set r [MapInitAffineConfTransf $projcs] }
    if { $r != 0 } {
	FixMapScale $proj
    }
    return $r
}

proc MapReduceCPoints {xys} {
    # reduce a set of points given by a list of their planar
    #  coordinates to 3 or 2
    # return list of indices in $xys of selected points or "" if less than 2
    #
    # reduction principle (suggested by Luis Damas): maximize the
    #  minimum distance between each 2 points in a triangle
    # algorithm: find the leftmost 3 edges forming a triangle
    #  in a list of triples with distance between two points and
    #  the 2 points indices, sorted by decreasing distance

    if { [set n [llength $xys]] < 2 } { return "" }
    if { $n == 2 } { return "0 1" }
    # form list of triples
    set ds ""
    set ix 0
    foreach p [lrange $xys 0 end-1] {
	foreach "x y" $p {}
	set ixi [expr $ix+1]
	foreach pi [lrange $xys 1 end] {
	    foreach "xi yi" $pi {}
	    set dx [expr $x-$xi] ; set dy [expr $y-$yi]
	    lappend ds [list [expr sqrt($dx*$dx+$dy*$dy)] $ix $ixi]
	    incr ixi
	}
	incr ix
    }
    set ds [lsort -decreasing -real -index 0 $ds]
    foreach t $ds {
	foreach "d ix0 ix1" $t {}
	# $ix0 < $ix1 by construction
	if { [catch {set s0 $seen($ix0)}] } {
	    set seen($ix0) $ix1
	    if { [catch {set seen($ix1)}] } {
		set seen($ix1) $ix0
	    } else { lappend seen($ix1) $ix0 }
	} elseif { [catch {set s1 $seen($ix1)}] } {
	    set seen($ix1) $ix0
	    lappend seen($ix0) $ix1
	} elseif { [set ix2 [Intersect1 $s0 $s1]] == "" } {
	    lappend seen($ix0) $ix1
	    lappend seen($ix1) $ix0
	} else {
	    # distance between $ix0 and $ix1 is the minimum of the
	    #  3 distances, therefore if the points are colinear
	    #  $ix2 is an extreme
	    break
	}	
    }
    foreach n "0 1 2" {
	foreach "x$n y$n" [lindex $xys [set ix$n]] {}
    }
    # colinear?
    if { $x0 == $x2 || $x1 == $x2 } {
	if { abs ($x0-$x1) < 10 } {
	    if { abs($y0-$y2) > abs($y1-$y2) } {
		return [list $ix2 $ix0]
	    }
	    return [list $ix2 $ix1]
	}
    } else {
	set dd02 [expr 1.0*($y0-$y2)/($x0-$x2)]
	set dd12 [expr 1.0*($y1-$y2)/($x1-$x2)]
	set ddm [expr { $dd02 > $dd12 } ? $dd02 : $dd12]
	if { abs(($dd02-$dd12)/$ddm) < 0.5 } {
	    if { abs($x0-$x2) > abs($x1-$x2) } {
		return [list $ix2 $ix0]
	    }
	    return [list $ix2 $ix1]
	}
    }
    return [list $ix2 $ix1 $ix0]
}

