#
#  gpsman --- GPS Manager: a manager for GPS receiver data
#
#  Copyright (c) 2000 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:  26 January 2000
#

## 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 these variables must be changed
# as well as MAPKNOWNTRANSFS set in main.tcl
# see also relevant procedures in map.tcl

     # indices of MTData array used for each transformation
set MAPTRANSFDATA(NoRot) {lambda e f}
set MAPTRANSFDATA(Affine) {a b c d e f det k2m1 k4m3}
set MAPTRANSFDATA(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 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 {} {
    # compute representation of affine transformation from 3 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 Mat Rx

    MapTransfIs Affine
    set tcs [MapGeoRefPoints 3]
    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 {} {
    # compute representation of affine conformal transformation from 2 WPs
    #  $MapLoadWPs is list of indices of relevant WPs
    #  $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"
    set tcs [MapGeoRefPoints 2]
    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
}

