#
#  gpsman --- GPS Manager: a manager for GPS receiver data
#
# Copyright (c) 1998-2008 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: gdata.tcl
#  Last change:  13 August 2007
#


## manipulation of geo-referenced data structures
#   consisting of a list of pairs $coord1 $col, in increasing order of $coord1
#   where $col is a list of pairs $coord2 $data, in increasing order of $coord2
#   and $data a list of information for each individual at $coord1 $coord2


proc AddSeqToGData {gdata coord1 coord2 info} {
    # add information to geo-referenced data structure
    # sequential search

    set ne [list $coord1 [list [list $coord2 [list $info]]]]
    set i 0
    foreach e $gdata {
	if { [set le [lindex $e 0]] > $coord1 } {
	    break
	} elseif { $le == $coord1 } {
	    set nee [list $coord2 [list $info]] ; set col [lindex $e 1]
	    set j 0
	    foreach ee $col {
		if { [set lee [lindex $ee 0]] > $coord2 } {
		    break
		} elseif { $lee == $coord2 } {
		    set nee [list $coord2 [linsert [lindex $ee 1] 0 $info]]
		    set col [lreplace $col $j $j $nee]
		    return [lreplace $gdata $i $i [list $coord1 $col]]
		}
		incr j
	    }
	    set col [linsert $col $j $nee]
	    return [lreplace $gdata $i $i [list $coord1 $col]]
	}
	incr i
    }
    return [linsert $gdata $i $ne]
}

proc AddToGData {gdata coord1 coord2 info} {
    # add information to geo-referenced data structure
    # binary search on top level list

    set ne [list $coord1 [list [list $coord2 [list $info]]]]
    if { [set b [llength $gdata]] == 0 } {
	return [list $ne]
    }
    set a 0
    while 1 {
	set i [expr ($a+$b)/2]
	set e [lindex $gdata $i]
	if { [set le [lindex $e 0]] > $coord1 } {
	    if { $a == $i } {
		break
	    }
	    set b $i
	} elseif { $le == $coord1 } {
	    set nee [list $coord2 [list $info]] ; set col [lindex $e 1]
	    set j 0
	    foreach ee $col {
		if { [set lee [lindex $ee 0]] > $coord2 } {
		    break
		} elseif { $lee == $coord2 } {
		    set nee [list $coord2 [linsert [lindex $ee 1] 0 $info]]
		    set col [lreplace $col $j $j $nee]
		    return [lreplace $gdata $i $i [list $coord1 $col]]
		}
		incr j
	    }
	    set col [linsert $col $j $nee]
	    return [lreplace $gdata $i $i [list $coord1 $col]]
	} elseif { $a == $i } {
	    incr a
	    break
	} else { set a $i }
    }
    return [linsert $gdata $a $ne]
}

proc LookupGData {gdata coord1 coord2} {
    # find information in geo-referenced data structure
    # return "" if nothing found
    # binary search on top level list

    if { [set b [llength $gdata]] == 0 } {
	return ""
    }
    set a 0
    while 1 {
	set i [expr ($a+$b)/2]
	set e [lindex $gdata $i]
	if { [set le [lindex $e 0]] > $coord1 } {
	    if { $a == $i } { break }
	    set b $i
	} elseif { $le == $coord1 } {
	    foreach ee [lindex $e 1] {
		if { [set lee [lindex $ee 0]] > $coord2 } {
		    return ""
		} elseif { $lee == $coord2 } {
		    return [lindex $ee 1]
		}
	    }
	    return ""
	} elseif { $a == $i } { break } else { set a $i }
    }
    return ""
}

proc LookupQuadrGData {gdata coord1 coord2 dc1 dc2} {
    # find information in geo-referenced data structure
    #  for each point with coordinates ($coord10,$coord20) if ($coord1,$coord2)
    #  belongs to the quadrangle centred on it and having as corners
    #  ($coord10-$dc1/2,$coord20-$dc2/2) ($coord10+$dc1/2,$coord20+$dc2/2)
    #  with $dc1 and $dc2 > 0
    # return list of information for each point
    # binary search on top level list

    if { $dc1 <= 0 || $dc2 <= 0 || [set b [llength $gdata]] == 0 } {
	return ""
    }
    set hdc1 [expr 0.5*$dc1] ; set hdc2 [expr 0.5*$dc2]
    set a 0 ; set res ""
    while 1 {
	set i [expr ($a+$b)/2]
	set e [lindex $gdata $i]
	if { [set le [lindex $e 0]]-$hdc1 > $coord1 } {
	    if { $a == $i } { return $res }
	    set b $i
	} elseif { $le+$hdc1 >= $coord1 } {
	    foreach ee [lindex $e 1] {
		if { [set lee [lindex $ee 0]]-$hdc2 > $coord2 } {
		    break
		} elseif { $lee+$hdc2 >= $coord2 } {
		    set res [concat $res [lindex $ee 1]]
		}
	    }
	    break
	} elseif { $a == $i } { break } else { set a $i }
    }
    for { set j [expr $i+1] } { $j != $b } { incr j } {
	set e [lindex $gdata $j]
	if { [set le [lindex $e 0]]-$hdc1 > $coord1 } { break }
	if {  $le+$hdc1 >= $coord1 } {
	    foreach ee [lindex $e 1] {
		if { [set lee [lindex $ee 0]]-$hdc2 > $coord2 } {
		    break
		} elseif { $lee+$hdc2 >= $coord2 } {
		    set res [concat $res [lindex $ee 1]]
		}
	    }
	}
    }
    incr a -1
    for { set j [expr $i-1] } { $j != $a } { incr j -1 } {
	set e [lindex $gdata $j]
	if { [set le [lindex $e 0]]+$hdc1 < $coord1 } { break }
	if {  $le-$hdc1 <= $coord1 } {
	    foreach ee [lindex $e 1] {
		if { [set lee [lindex $ee 0]]-$hdc2 > $coord2 } {
		    break
		} elseif { $lee+$hdc2 >= $coord2 } {
		    set res [concat $res [lindex $ee 1]]
		}
	    }
	}
    }
    return $res
}

#### geo-referencing time-stamped information

proc GeoAdapt {type ix datum lts} {
    # geo-reference time-stamped information in list $lts adapting it to
    #  item of given $type and index $ix
    # if $type==TR, interpolate/extrapolate positions from time-stamps of TPs
    # if $type==GR, take each WP in sequence from GR after sorting time-stamps
    #  $lts is a list of tuples whose first element is the time-stamp in
    #       seconds from $YEAR0, sorted by time-stamps
    #  $datum is the datum for the computed positions
    # return "" on error, or list with tuples obtained from the tuples in $lst
    #  by inserting the latitude and longitude (decimal degrees, for $datum)
    #  and altitude (metre) at the beginning
    global TRTPoints TRDatum DataIndex GRConts WPPosn WPDatum WPAlt

    set glts ""
    if { $type == "TR" } {
	foreach v "la lo s alt" n "TPlatd TPlongd TPsecs TPalt" {
	    set ${v}ix $DataIndex($n)
	    set prev$v undef
	}
	if { $TRDatum($ix) != $datum } {
	    set tps [ChangeTPsDatum $TRTPoints($ix) $TRDatum($ix) $datum]
	} else { set tps $TRTPoints($ix) }
	set tp [lindex $tps 0] ; set tps [lreplace $tps 0 0]
	foreach x "la lo s alt" {
	    set tpt$x [lindex $tp [set ${x}ix]]
	}
	set tptalt [lindex $tptalt 0]
	foreach tuple $lts {
	    set secs [lindex $tuple 0]
	    while { $secs > $tpts && $tps != "" } {
		set tp [lindex $tps 0] ; set tps [lreplace $tps 0 0]
		foreach x "la lo s alt" {
		    set prev$x [set tpt$x]
		    set tpt$x [lindex $tp [set ${x}ix]]
		}
		set tptalt [lindex $tptalt 0]
	    }
	    # default to postion of current TP if in trouble
	    #  this may imply several tuples to have the same position
	    set la $tptla ; set lo $tptlo ; set alt $tptalt
	    if { $secs != $tpts } {
		if { $prevla != "undef" } {
		    # liner interpolation or extrapolation
		    if { ! [catch \
			   {set ratio [expr 1.0*($secs-$prevs)/($tpts-$prevs)]}]
		     } {
			set la [expr $prevla+$ratio*($tptla-$prevla)]
			set lo [expr $prevlo+$ratio*($tptlo-$prevlo)]
			if { $tptalt == "" || $prevalt == "" } {
			    set alt ""
			} else {
			    set alt [expr $prevalt+$ratio*($tptalt-$prevalt)]
			}
		    }
		} elseif { $secs < $tpts && $tps != "" } {
		    set tp2 [lindex $tps 0]
		    foreach x "la lo s alt" {
			set tp2$x [lindex $tp2 [set ${x}ix]]
		    }
		    set tp2alt [lindex $tp2alt 0]
		    if { ! [catch \
			    {set ratio [expr 1.0*($secs-$tp2s)/($tpts-$tp2s)]}]
		     } {
			set la [expr $tp2la+$ratio*($tptla-$tp2la)]
			set lo [expr $tp2lo+$ratio*($tptlo-$tp2lo)]
			if { $tptalt == "" || $tp2alt == "" } {
			    set alt ""
			} else {
			    set alt [expr $tp2alt+$ratio*($tptalt-$tp2alt)]
			}
		    }
		}
	    }
	    lappend glts [linsert $tuple 0 $la $lo $alt]
	}
    } else {
	# $type == "GR"
	set wpns ""
	foreach p $GRConts($ix) {
	    if { [lindex $p 0] == "WP" } {
		set wpns [lindex $p 1]
		break
	    }
	}
	if { $wpns == "" } {
	    GMMessage $MESS(allundef)
	    return ""
	}
	foreach tuple $lts {
	    set secs [lindex $tuple 0]
	    while { $wpns != "" && \
			[set wpix [IndexNamed WP [lindex $wpns 0]]] == -1 } {
		GMMessage [format $MESS(undefinedWP) [lindex $wpns 0]]
		set wpns [lreplace $wpns 0 0]
	    }
	    if { $wpns == "" } {
		# $la, ...  are defined because $wpns was initially non-empty
		# not enough WPs: use last one for the remaining tuples
		lappend glts [linsert $tuple 0 $la $lo $alt]
		continue
	    }
	    set la [lindex $WPPosn($wpix) 0] ; set lo [lindex $WPPosn($wpix) 1]
	    if { $WPDatum($wpix) != $datum } {
		set p [ToDatum $la $lo $WPDatum($wpix) $datum]
		set la [lindex $p 0] ; set lo [lindex $p 1]
	    }
	    set alt [lindex $WPAlt($ix) 0]
	    lappend glts [linsert $tuple 0 $la $lo $alt]
	    set wpns [lreplace $wpns 0 0]
	}
    }
    return $glts
}

