#! /usr/bin/env elwish
#
# Copyright (c) 2008-2010 CNRS/LAAS
#
# Permission to use, copy, modify, and distribute this software for any
# purpose with or without fee is hereby granted, provided that the above
# copyright notice and this permission notice appear in all copies.
#
# THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
# WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
# MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
# ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
# WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
# ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
# OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
#
# Matthieu Herrb - Mon Sep  1 2008
#
lappend auto_path [file dirname $argv0]

package require xml
package require angle
package require genom
package require Gdhe

if {![info exists picoHost]} {
    set picoHost localhost
}

# Generic way to start a Unix daemon
proc startDaemon { d } {
    if {[catch {exec $d >@stdout 2>/dev/null} result]} {
	puts "$::errorCode"
    }
}

# Genreric way to start a Genom module 
proc startModule { m } {
    if {[catch {exec $m -b >@stdout 2>@stderr} result]} {
	puts "$::errorCode"
    }
}

# locoDemo display methods
namespace eval locoDemo {
    global pos
    global picoHost

    variable posterBase "http://$picoHost:8080/locoDemo?get="
    variable url
    variable ok 0

    proc init {} {
	variable posterBase
	variable url
	variable ok

	set url $posterBase
	append url "MEPos"

	# Try to read the poster once 
	if {[catch {xml::getDoc $url} doc] != 0} {
	    puts "locoDemo::init: $doc"
	} else {
	    puts "locoDemo::init ok"
	    set ok 1
	} 
	catch { $doc delete }
    }

    proc update {var index} {
	variable posterBase
	variable url
	variable ok

	upvar \#0 $var p
	if {$ok == 0} { return }
	if {[catch {xml::getDoc $url} doc] != 0} {
	    puts "locoDemo::update $doc"
	    return
	}
	
	set x [[$doc selectNodes "//mePos/main/euler/x/text()"] data ]
	set y [[$doc selectNodes "//mePos/main/euler/y/text()"] data ]
	set theta [angle::rad2deg [[$doc selectNodes "//mePos/main/euler/yaw/text()"] data ]]
	
	set p($index) "$x $y $theta"
	redrawAllWindows
	
	catch { $doc delete }
    }
}

# Display update loop
proc loop {} {
	      global stopLoop
	      locoDemo::update pos r
	      if {$stopLoop == 0} {
		  after 10 loop
	      }
}


# Module init code
proc init {} {
    global stopLoop
    global trackId
    
    pilo::Init locoDemoMEPos
    set trackId [locoDemo::Track -ack piloPosRef]

    locoDemo::init
    locoDemo::update pos r

    set stopLoop 0
    after 10 loop
}

# Stop whole demo
proc end {} {
    global stopLoop
    global trackId
    
    set stopLoop 1
    locoDemo::Stop
    catch {replyof $trackId} result
    kill pilo
    kill locoDemo
    if {[catch {exec pkill picoweb} result]} {
	puts "pkill picoweb: $::errorCode"
    }
    die
}

# simple move command
proc m {d} {
    pilo::Move $d 80 80
}

# simple turn command
proc t {a} {
    pilo::Turn [angle::deg2rad $a] 0 80 80
}


#
# main code
#

#  start servers and modules
startDaemon tclserv
startDaemon picoweb
startModule locoDemo
startModule pilo

# init GDHE objects
set robots(R) { color 200 0 0 ; repere 1 }
set pos(R) { 0 0 0 }

set pos(r) { 0 0 0 }
set robots(r) { atrv }

# connect to modules
connect localhost
lm locoDemo
lm pilo

# Setup modules
init

# switch to interactive mode...
interactive
