#
#  gpsman --- GPS Manager: a manager for GPS receiver data
#
#  Copyright (c) 2005 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: garmin_nmea.tcl
#  Last change:  22 November 2005
#

## procedures used to interpret NMEA-0183 sentences from a receiver or
##  a file (see files_foreign.tcl)

### Some NMEA-0183 sentences (check proc CollectNMEAData for the information
#    actually used)
# GPRMC Recommended minimum specific GPS/Transit data
#        [course made good assumed to give current bearing]
# GPRMB Recommended minimum navigation information (active destination)
# GPGGA GPS fix data
# GPGLL Latitude and longitude
# GPGSA GPS DOP and Active Satellites
#   (3) Number of Satellites used in solution
#   PDOP (4) Position dilution of precision 1.0 to 99.9
#   HDOP (5) Horizontal DOP 1.0 to 99.9
#   VDOP (6) Vertical DOP 1.0 to 99.9
# GPGSV GPS Satellites in View
#   (3) Number of Satellites in view
#   (4) Satellites ID (PRN)
#   (5) Elevations (degrees) 00 to 90
#   (6) Azimuths (true) (degrees) 000 to 359
#   (7) Signal to noise ratios 00 to 99 dB
# GPBOD Bearing from origin to destination
# GPBWC Bearing and distance to destination along great circle
# GPBWR Bearing and distance to destination along rhumb line
# GPVTG Velocity and track made good [assumed to give current bearing]
# GPXTE Cross-track error
# PGRMC Garmin Configuration Information
#   (1) Fix Mode 2D/3D/Automatic
#   (3) Earth Datum Index
#   (4-9) User defined Earth Datum a/f/dx/dy/dz
#   (11) Velocity Filter Automatic/2-256 seconds
# PGRME Garmin EPE
# PGRMF Garmin GPS Fix Data Sentence 
# PGRMI Garmin Initiallzation Information
# PGRMM Garmin datum
# PGRMO Garmin Output Sentence Enable/Disable
# PGRMT Garmin Sensor Status Information
# PGRMV Garmin 3D Velocity Information non-standard
#   (1) True east velocity (m/sec) -999.9 to 9999.9
#   (2) True north velocity (m/sec) -999.9 to 9999.9
#   (3) Up velocity (m/sec) -999.9 to 9999.9
# PGRMZ Garmin altitude


  ## changes here will affect proc CollectNMEAData
array set NMEASent {
    GPRMC  {time fix lat long knot-speed bear date magnvar}
    GPRMC,tp {time status=V/A DMM,N/S DMM,E/W float float date float}
    GPRMB  {fix nm-XTE TRNdir WPStart WPName WPlat WPlong nm-WPdist
            bearWP knot-velWP prox}
    GPRMB,tp {status=V/A float<9.9 L/R string string DMM,N/S DMM,E/W float
              float float V/A}
    GPGGA  {time lat long fix nsat HDOP alt gheight DGPSdt DGPSid}
    GPGGA,tp {time DMM,N/S DMM,E/W status=0/1/2 int float float,=M
              float,=M float int}
    GPGLL  {lat long time sel}
    GPGLL,tp {DMM,N/S DMM,E/W time status=?/A}
    GPBOD  {bearStartDest magnbearStartDest WPDest WPStart}
    GPBOD,tp {float,=T float,=M string string}
    GPBWC  {time WPlat WPlong bearWP magnbearWP nm-WPdist WPname}
    GPBWC,tp {time DMM,N/S DMM,E/W float float float string}
    GPBWR  {time WPlat WPlong rhumbWP magnrhumbWP nm-WPdist WPname}
    GPBWR,tp {time DMM,N/S DMM,E/W float float float string}
    GPVTG  {bear magnbear knot-speed kph-speed}
    GPVTG,tp {float,=T float,=M float float}
    GPXTE  {XTE TRNdir XTEunit}
    GPXTE,tp {float L/R N/K/M}
    PGRME  {EPH EPV EPE}
    PGRME,tp {float,=M float,=M float,=M}
    PGRMZ  {ft-alt fixalt}
    PGRMZ,tp {float,=f status=?/2/3}
    PGRMM  datum
    PGRMM,tp string
}

set NMEAKnown [array names NMEASent]

array set NMEAStatus {
    0  _
    1  GPS
    2  DGPS
    A  Auto
    V  _
    alt=2  Manual
    alt=3  GPS
}

set NMEARLTM 0

### user control

proc GarminStartNMEA {from args} {
    # start interpretation of NMEA 0183 sentences
    #  $from in {rec, file} for, respectively,
    #    real time logging from receiver
    #    reading from a file $args
    # return 0 on failure
    global NMEARLTM NMEAData NMEACurrentTime NMEACurrentDate NMEASeen \
	    NMEAFrom NMEADataFile NMEADefDate NMEAPrevSecs

    if { ! $NMEARLTM } {
	set NMEAData  "_ _ _ _ _ _ _ _ _ _"
	set NMEASeen "" ; set NMEACurrentTime undef
	set NMEACurrentDate ""
	if { [set NMEAFrom $from] == "rec" } {
	    set NMEADefDate [clock format [clock seconds] -format "%Y %m %d"]
	    set NMEAPrevSecs -2
	    return [set NMEARLTM [StartLineProtocol ProcNMEALine 4800]]
	}
	set NMEADefDate "1990 1 1" ; set NMEAPrevSecs -1
	set NMEADataFile $args
	set NMEARLTM 1
    } elseif { $NMEAFrom != $from || $from == "file" } {
	return 0
    }
    return 1
}

proc GarminStopNMEA {} {
    global NMEARLTM

    StopLineProtocol
    set NMEARLTM 0
    return
}

### low-level line processing

proc ProcNMEALine {line lxor} {
    # process a NMEA sentence
    #  $lxor is the XOR of all chars in $line (as string)
    # return 1 on error
    global NMEAKnown NMEASent NMEAInvalid

    if { [regexp {[^ -~]+} $line] } {
	Log "PNL> bad chars in: $line"
	return 1
    }
    if { [string index $line 0] != "\$" } {
	Log "PNL> no starting dollar in: $line"
	return 1
    }
    set fs [split [string range $line 1 end] ","]
    set last [lindex $fs end]
    if { [regexp {(.*)\*(..)} $last z field chksum] } {
	if { [scan $chksum "%x" csval] } {
	    # take from $lxor the codes of "$", "*" and the checksum digits
	    binary scan [string index $chksum 0] "c" d1
	    binary scan [string index $chksum 1] "c" d2
	    foreach code "36 42 $d1 $d2" { set lxor [expr $lxor ^ $code] }
	    if { $lxor != $csval } {
		Log "PNL> wrong checksum ($lxor not $csval) in: $line"
		return 1
	    }
	} else {
	    Log "PNL> bad checksum field in: $line"
	    return 1
	}
	set fs [lreplace $fs end end $field]
    }
    set sent [lindex $fs 0]
    if { [lsearch -exact $NMEAKnown $sent] == -1 } {
	# Log "PNL> discarding: $line"
    } else {
	set NMEAInvalid 0
	set data "" ; set time undef
	set ix 1
	foreach attr $NMEASent($sent) type $NMEASent($sent,tp) {
	    set fld [lindex $fs $ix] ; incr ix
	    if { [regexp {.+,.+} $type] } {
		set annex [lindex $fs $ix] ; incr ix
	    } else { set annex "" }
	    set val [DecodeNMEAField $type $fld $annex]
	    if { $NMEAInvalid } { return 1 }
	    lappend data $val
	    if { $attr == "time" } { set time $val }
	}
	CollectNMEAData $sent $time $data
    }
    return 0
}

proc DecodeNMEAField {type field annex} {
    # decode field in NMEA sentence
    # return "_" if field is empty
    # set NMEAInvalid on error or if the fix status is invalid
    global NMEAInvalid

    if { $field == "" } {
	if { [regexp {^status=} $type] } {
	    # Log "DNF> invalid fix; discarding sentence"
	    set NMEAInvalid 1
	}
	return "_"
    }
    switch -glob $type {
	DMM,?/? {
	    # DMM and heading letter -> signed degrees
	    regexp {DMM,(.)/(.)} $type z ph nh
	    if { $annex == $ph } {
		set sign 1
	    } elseif { $annex == $nh } {
		set sign -1
	    } else {
		Log "DNF> bad sign in $type: $annex, field is $field"
		set NMEAInvalid 1 ; return 0
	    }
	    if { ! [regexp {0*([1-9]*[0-9]*)([0-9])([0-9]\.[0-9]+)} $field \
		    z d m1 m2] } {
		Log "DNF> bad DMM value: $field"
		set NMEAInvalid 1 ; return 0
	    }
	    if { $d == "" } { set d 0 }
	    if { $m1 == 0 } { set m $m2 } else { set m [expr 10*$m1+$m2] }
	    return [expr $sign*($d+$m/60.0)]
	}
	*,=* {
	    regexp {(.+),=(.+)} $type z t1 str
	    if { $annex == $str } {
		return [DecodeNMEAField $t1 $field ""]
	    }
	    Log "DNF> bad sub-field of type $type: $annex, for field $field"
	}
	status=* {
	    # fix status, first possible value is for error
	    regsub {status=} $type "" vals
	    set vals [split $vals "/"]
	    if { [set ix [lsearch -exact $vals $field]] != -1 || \
		    [set ix [lsearch -exact $vals "?"]] != -1 } {
		if { $ix == 0 } { set NMEAInvalid 1 }
		return $field
	    }
	    Log "DNF> bad or unknown value for $type: $field"
	}
	*/* {
	    # choice of values
	    set vals [split $type "/"]
	    if { [lsearch -exact $vals $field] != -1 } { return $field }
	    Log "DNF> bad or unknown value for $type: $field"
	}
	time {
	    # HHMMSS -> [list h mn s]
	    return [DecodeNMEATimeDate $field "23 59 59"]
	}
	date {
	    # DDMMYY -> [list d m yy]
	    return [DecodeNMEATimeDate $field "31 12 99"]
	}
	float {
	    if { [scan $field %f val] == 1 } { return $val }
	    Log "DNF> bad float: $field"
	}
	float<* {
	    if { [scan $field %f val] == 1 } {
		regsub {float<} $type "" mx
		if { $val >= $mx } { set val Inf }
		return $val
	    }
	    Log "DNF> bad float with limit: $field"
	}
	int {
	    if { [scan $field %d val] == 1 } { return $val }
	    Log "DNF> bad int: $field"
	}
	string {
	    return $field
	}
	default {
	    BUG "bad specification for type of NMEA field"
	}
    }
    set NMEAInvalid 1
    return 0
}

proc DecodeNMEATimeDate {td mxs} {
    # HHMMSS or DDMMYY into list
    # if $td has a "." then it is taken to be in HHMMXX._ format
    #  where XX._ is seconds in %0f format
    #  $mxs are extreme values for each sub-field
    # set NMEAInvalid on error
    global NMEAInvalid

    if { [string length $td] == 6 } {
	set fmt %02d%02d%02d
    } elseif { [regexp {\.} $td] } {
	set fmt %02d%02d%0f
    } else { set fmt %1d%02d%02d }
    if { [scan $td $fmt s1 s2 s3] == 3 } {
	set s3 [expr round($s3)]
	foreach v "s1 s2 s3" mx $mxs {
	    if { [set $v] > $mx } {
		Log "DNTD> bad time or date: [set $v] in $td"
		set NMEAInvalid 1 ; return 0
	    }
	}
	return [list $s1 $s2 $s3]
    }
    Log "DNTD> bad time or date: $td"
    set NMEAInvalid 1
    return 0
}

### collect data

proc CollectNMEAData {sent time data} {
    # collect data from sentence $sent with given time stamp
    #  $data is a list of values for the attributes described
    #   in the NMEASent array; their positions are used here explicitly
    # the list $NMEAData contains (any element may be "_" for undefined):
    # 0    - latitude in degrees (assumed datum is "WGS 84")
    # 1    - longitude in degrees
    # 2    - position fix: either in {1, 2} for {GPS, DGPS}, or "A" for ok
    # 3    - EPE in metres
    # 4    - EPH im metres
    # 5    - EPV in metres
    # 6    - altitude in metres
    # 7    - speed in km/h
    # 8    - bearing
    # collected data is sent to upper level only when a new time stamp
    #  or a repeated sentence is got
    global NMEAData NMEASeen NMEACurrentTime NMEACurrentDate

    if { [lsearch -exact $NMEASeen $sent] != -1 } {
	SendNMEAData $time
    } elseif { $time != "undef" } {
	if { $NMEACurrentTime == "undef" } {
	    set NMEACurrentTime $time
	} elseif { $time != $NMEACurrentTime } {
	    SendNMEAData $time
	}
    }
    switch $sent {
	GPGGA {
	    set from "1 2 3 6" ; set to "0 1 2 6"
	}
	GPGLL {
	    set from "0 1 3" ; set to "0 1 2"
	}
	GPRMC {
	    set sp [expr [lindex $data 4]/539.9568e-3]
	    set data [lreplace $data 4 4 $sp]
	    set date [lindex $data 6]
	    if { $date != "_" } {
		set NMEACurrentDate $date
	    }
	    set from "1 2 3 4 5" ; set to "2 0 1 7 8"
	}
	GPVTG {
	    set from "0 3" ; set to "8 7"
	}
	PGRME {
	    set from "0 1 2" ; set to "4 5 3"
	}
	PGRMZ {
	    if { [lindex $NMEAData 6] != "_" } {
		set data [expr round([lindex $data 0]*0.3048)]
		set from 0 ; set to 6
	    } else {
		set from ""
	    }
	}
	default {
	    set from ""
	}
    }
    if { $from != "" } { MergeNMEAData $from $data $to }
    return
}

proc MergeNMEAData {frixs data toixs} {
    # merge data items indexed by $frixs with NMEAData at indices $toixs
    # undefined items are discarded
    global NMEAData

    foreach from $frixs to $toixs {
	if { [set it [lindex $data $from]] != "_" } {
	    set NMEAData [lreplace $NMEAData $to $to $it]
	}
    }
    return
}

proc SendNMEAData {time} {
    # send data collected to upper level if current time is defined
    global NMEAStatus NMEAData NMEASeen NMEACurrentTime NMEACurrentDate \
	    NMEAFrom NMEADataFile NMEADefDate NMEAPrevSecs

    if { $NMEACurrentTime != "undef" } {
	foreach "h m s" $NMEACurrentTime {}
	set tsecs [expr $h*3600+$m*60+$s]
	if { $NMEACurrentDate == "" } {
	    if { $NMEAPrevSecs > $tsecs && [lindex $NMEADefDate 0] != 1990 } {
		# $NMEAPrevSecs >= 0 and over midnight
		foreach "y m d" $NMEADefDate {}
		set NMEADefDate [clock format \
			[clock scan "1 day" \
			-base [clock scan ${y}-${m}-${d}]] -format "%Y %m %d"]
	    }
	} else {
	    if { [scan $NMEACurrentDate "%d %d %d" d m y] != 3 } {
		BUG "bad current date in SendNMEAData: $NMEACurrentDate"
	    }
	    if { $y < 88 } {
		# safe up to 2087...
		incr y 2000
	    } else { incr y 1900 }
	    set NMEADefDate "$y $m $d"
	}
	set NMEACurrentDate ""
	if { $NMEAPrevSecs != -2 } { set NMEAPrevSecs $tsecs }
	set date [concat $NMEADefDate $NMEACurrentTime]
	set sp [lindex $NMEAData 7]
	if { [set bear [lindex $NMEAData 8]] == "_" || $sp == "_" } {
	    set vx "_" ; set vy "_"
	} else {
	    set a [expr $bear*0.01745329251994329576]
	    set vx [expr $sp*cos($a)/1000.0]
	    set vy [expr $sp*sin($a)/1000.0]
	}
	set rltmdata [list $date \
		[list [lindex $NMEAData 0] [lindex $NMEAData 1]] \
		$NMEAStatus([lindex $NMEAData 2]) \
		[lrange $NMEAData 3 5] \
		[lindex $NMEAData 6] \
		[list $vx $vy _] \
		$bear "_" "_" $sp]
	if { $NMEAFrom == "rec" } {
	    after 0 [list UseRealTimeData $rltmdata]
	} else {
	    ImportNMEAData $rltmdata $NMEADataFile
	}
    }
    if { $NMEAFrom == "rec" } {
	set NMEADefDate [clock format [clock seconds] -format "%Y %m %d"]
    }
    set NMEAData "_ _ _ _ _ _ _ _ _"
    set NMEASeen "" ; set NMEACurrentTime $time
    return
}

