##############################################################################
# Copyright 2003 MBARI.                                                      #
# Monterey Bay Aquarium Research Institute Proprietary Information.          #
# All rights reserved.                                                       #
##############################################################################
#package require udp
set pilot_interface .pilot_interface

source serv_frame.tcl      
source mode_frame.tcl      
source jog_frame.tcl
source move_frame.tcl
source offset_frame.tcl
source status_frame.tcl

#set sock [udp_listen 142]
#vwait forever
#close $sock
# set the status labels and lights positions

proc pilot_interface { } {
	
	global pilot_interface
	global sliders
        global env
	set env(DISPLAY) :0.1

	# Dave - April. 16, 2003
	if [winfo exists .pilot_interface] {destroy .pilot_interface}	
	toplevel .pilot_interface
	wm title . "Pilot Interface - Stanford ARL Precision Control"


#******** frame for the server entry controls******

frame .pilot_interface.serv_frm -relief groove -borderwidth 1

# create the widgets for the form
#label $pilot_interface.serv_frm.title_lbl -text "Keyboard server entry" -font {Halvetica 10}

label $pilot_interface.serv_frm.server_lbl -text "Server: " -font {Halvetica 10} 		
entry $pilot_interface.serv_frm.server_ent -textvariable server_input -width 18

#-validate key -vcmd {string is double %P}

label $pilot_interface.serv_frm.port_lbl -text "Port: " -font {Halvetica 10} 	
entry $pilot_interface.serv_frm.port_ent -textvariable port_input -width 10
#-vcmd {string is integer %P}
#-validate key
button $pilot_interface.serv_frm.conn_btn -text "  Connect  " -font {Halvetica 10} \
       -command connect_cmd

# layout the widgets on the form

grid $pilot_interface.serv_frm.server_lbl  -row 1 -column 0 -pady 0
grid $pilot_interface.serv_frm.server_ent  -row 1 -column 1 -padx 0 -pady 0
grid $pilot_interface.serv_frm.port_lbl    -row 1 -column 2 -pady 7
grid $pilot_interface.serv_frm.port_ent    -row 1 -column 3 -padx 10 -pady 7
grid $pilot_interface.serv_frm.conn_btn    -row 1 -column 4 -ipadx 5 -ipady 5 -pady 7

global lxa lxb lxc lya lyb
global sxa1 sxa2 sxb1 sxb2 sxc1 sxc2 sya1 sya2 syb1 syb2

#***** canvas for status feedback*****
canvas $pilot_interface.status_can -width 350 -height 35  -relief groove -borderwidth 3

# create the status labels and lights, and place them on the canvas

label $pilot_interface.status_can.con_lab -text "Connected" -font {Halvetica 10}
place $pilot_interface.status_can.con_lab -x $lxa -y $lya

label $pilot_interface.status_can.mov_lab -text "Moving" -font {Halvetica 10}
place $pilot_interface.status_can.mov_lab -x $lxa -y $lyb

label $pilot_interface.status_can.mid_lab -text "Midwater" -font {Halvetica 10}
place $pilot_interface.status_can.mid_lab -x $lxb -y $lya

label $pilot_interface.status_can.ben_lab -text "Benthic" -font {Halvetica 10}
place $pilot_interface.status_can.ben_lab -x $lxb -y $lyb

label $pilot_interface.status_can.dvl_lab -text "DVL Status" -font {Halvetica 10}
place $pilot_interface.status_can.dvl_lab -x $lxc -y $lya

label $pilot_interface.status_can.spr_lab -text "Spare Status" -font {Halvetica 10}
place $pilot_interface.status_can.spr_lab -x $lxc -y $lyb

#******* frame for the mode buttons******

frame $pilot_interface.mode_frm -relief groove -borderwidth 3

#create the mode buttons and layout on grid
button $pilot_interface.mode_frm.idle_btn      -width 8 -text "Idle" -font {Halvetica 10} \
       -command idle_cmd
button $pilot_interface.mode_frm.mid_water_btn -width 8 -text "Midwater" -font {Halvetica 10} \
       -command mid_water_cmd
button $pilot_interface.mode_frm.benthic_btn   -width 8 -text "Benthic" -font {Halvetica 10} \
       -command benthic_cmd

grid $pilot_interface.mode_frm.idle_btn      -row 0 -column 0 -ipadx 5 -ipady 5 -padx 5 -pady 7
grid $pilot_interface.mode_frm.mid_water_btn -row 0 -column 1 -ipadx 5 -ipady 5 -padx 5 -pady 7
grid $pilot_interface.mode_frm.benthic_btn   -row 0 -column 2 -ipadx 5 -ipady 5 -padx 5 -pady 7

#******* frame for the jog controller and scale controls*****

frame $pilot_interface.jog_frm -relief groove -borderwidth 3
frame $pilot_interface.jog_frm.ctrlr_frm -borderwidth 5
frame $pilot_interface.jog_frm.scale_frm -borderwidth 5

# create the buttons for all the directions and place on grid
button $pilot_interface.jog_frm.ctrlr_frm.arrow_nw_btn -image arrow_nw -command jog_nw
button $pilot_interface.jog_frm.ctrlr_frm.arrow_n_btn  -image arrow_n -command jog_n
button $pilot_interface.jog_frm.ctrlr_frm.arrow_ne_btn -image arrow_ne -command jog_ne

button $pilot_interface.jog_frm.ctrlr_frm.arrow_w_btn  -image arrow_w -command jog_w
button $pilot_interface.jog_frm.ctrlr_frm.stop_btn     -image stop_sign -command stop_cmd
button $pilot_interface.jog_frm.ctrlr_frm.arrow_e_btn  -image arrow_e -command jog_e

button $pilot_interface.jog_frm.ctrlr_frm.arrow_sw_btn -image arrow_sw -command jog_sw
button $pilot_interface.jog_frm.ctrlr_frm.arrow_s_btn  -image arrow_s -command jog_s
button $pilot_interface.jog_frm.ctrlr_frm.arrow_se_btn -image arrow_se -command jog_se

grid $pilot_interface.jog_frm.ctrlr_frm.arrow_nw_btn  -row 0 -column 0
grid $pilot_interface.jog_frm.ctrlr_frm.arrow_n_btn   -row 0 -column 1
grid $pilot_interface.jog_frm.ctrlr_frm.arrow_ne_btn  -row 0 -column 2

grid $pilot_interface.jog_frm.ctrlr_frm.arrow_w_btn   -row 1 -column 0
grid $pilot_interface.jog_frm.ctrlr_frm.stop_btn      -row 1 -column 1 -padx 5 -pady 5
grid $pilot_interface.jog_frm.ctrlr_frm.arrow_e_btn   -row 1 -column 2

grid $pilot_interface.jog_frm.ctrlr_frm.arrow_sw_btn  -row 2 -column 0
grid $pilot_interface.jog_frm.ctrlr_frm.arrow_s_btn   -row 2 -column 1
grid $pilot_interface.jog_frm.ctrlr_frm.arrow_se_btn  -row 2 -column 2

# create scale sliders and buttons and place on a grid
# the radio buttons just assign the incr_var
radiobutton $pilot_interface.jog_frm.scale_frm.dist_rb -height 3 -width 7 -anchor w \
    -text "Distance\n(m)"  -font {Halvetica 10} -variable inc_var -value dist
radiobutton $pilot_interface.jog_frm.scale_frm.rate_rb -height 3 -width 7 -anchor w \
    -text "Rate\n(m/s)" -font {Halvetica 10} -variable inc_var -value slew_rate

# set up the rate and distance scale sliders
scale $pilot_interface.jog_frm.scale_frm.dist_scale -from 0 -to 25 -width 30 -length 230 \
    -font {Halvetica 10} -variable dist -orient horizontal -tickinterval 5 \
    -showvalue true -resolution 0.1

scale $pilot_interface.jog_frm.scale_frm.rate_scale -from 0 -to 1 -width 30 -length 230 \
    -font {Halvetica 10} -variable slew_rate -orient horizontal \
    -tickinterval 0.2 -showvalue true -resolution 0.005

# bind the dist and rate scales to the incr_var
bind $pilot_interface.jog_frm.scale_frm.dist_scale <1> { set inc_var dist }
bind $pilot_interface.jog_frm.scale_frm.rate_scale <1> { set inc_var slew_rate }

# create the incr/decr buttons
button $pilot_interface.jog_frm.scale_frm.apply_btn -text "Apply\nRate" -font {Halvetica 10} \
       -command rate_cmd

button $pilot_interface.jog_frm.scale_frm.decr_big_btn   -image decr_big \
    -command { dec_big $inc_var }
button $pilot_interface.jog_frm.scale_frm.decr_small_btn -image decr_small \
    -command { dec_small $inc_var }
button $pilot_interface.jog_frm.scale_frm.incr_small_btn -image incr_small \
    -command { inc_small $inc_var }
button $pilot_interface.jog_frm.scale_frm.incr_big_btn   -image incr_big \
    -command { inc_big $inc_var }

# lay out the tools on the grid
grid $pilot_interface.jog_frm.scale_frm.dist_rb        -row 1 -column 0
grid $pilot_interface.jog_frm.scale_frm.dist_scale     -row 1 -column 1 -columnspan 4
grid $pilot_interface.jog_frm.scale_frm.apply_btn      -row 2 -column 0 -ipadx 7 -sticky ns
grid $pilot_interface.jog_frm.scale_frm.decr_big_btn   -row 2 -column 1 -padx 5
grid $pilot_interface.jog_frm.scale_frm.decr_small_btn -row 2 -column 2 -padx 5
grid $pilot_interface.jog_frm.scale_frm.incr_small_btn -row 2 -column 3 -padx 5
grid $pilot_interface.jog_frm.scale_frm.incr_big_btn   -row 2 -column 4 -padx 5
grid $pilot_interface.jog_frm.scale_frm.rate_rb        -row 3 -column 0
grid $pilot_interface.jog_frm.scale_frm.rate_scale     -row 3 -column 1 -columnspan 4


# layout both sub grids on a larger grid

grid $pilot_interface.jog_frm.ctrlr_frm -row 0 -column 1
grid $pilot_interface.jog_frm.scale_frm -row 1 -column 1


#***** frame for the offset script ****
frame $pilot_interface.offset_frm -relief groove -borderwidth 3

# set up slider bars
set sliders(x) [offset_slider x ]
set sliders(y) [offset_slider y horizontal]

button $pilot_interface.offset_frm.zero_btn -width 8 -text "Zero" -font {Halvetica 10} \
-command zero_position_offset
button $pilot_interface.offset_frm.jog_btn -width 8 -text "Move" -font {Halvetica 10} \
-command jog_to_position_offset

# Grid layout
global sliders

grid $sliders(x) -row 0
grid $sliders(y) -row 1
grid $pilot_interface.offset_frm.zero_btn -row 2 -ipadx 5 -ipady 5 -padx 5 -pady 7
grid $pilot_interface.offset_frm.jog_btn  -row 3 -ipadx 5 -ipady 5 -padx 5 -pady 7


#***** frame for the bearing entry controls *****
frame $pilot_interface.move_frm -relief groove -borderwidth 3

# create the widgets for the form

label $pilot_interface.move_frm.bearing_lbl -text "Heading \n(deg) " -font {Halvetica 10} 		
entry $pilot_interface.move_frm.bearing_ent -textvariable bearing_input -width 12
      	#-validate key-vcmd {string is double %P}

label $pilot_interface.move_frm.dist_lbl -text "Distance \n(m) " -font {Halvetica 10} 	
entry $pilot_interface.move_frm.dist_ent -textvariable dist_input -width 12
	#-validate key -vcmd {string is double %P}

button $pilot_interface.move_frm.move_btn -text "Move ROV" -font {Halvetica 10} \
       -command move_cmd

# layout the widgets on the form

grid $pilot_interface.move_frm.bearing_lbl -row 0 -column 0 -pady 7
grid $pilot_interface.move_frm.bearing_ent -row 0 -column 1 -padx 10 -pady 7
grid $pilot_interface.move_frm.dist_lbl    -row 0 -column 2 -pady 7
grid $pilot_interface.move_frm.dist_ent    -row 0 -column 3 -padx 10 -pady 7
grid $pilot_interface.move_frm.move_btn    -row 0 -column 4 -ipadx 5 -ipady 5 -pady 7



# create a spacer frame to keep the other frames pushed away from the menu bar
frame $pilot_interface.line_frm -height 15

#
# pack the control frames in the main frame 
#
grid $pilot_interface.line_frm   -row 0 -column 0 -columnspan 2 -sticky news
grid $pilot_interface.serv_frm   -row 1 -column 0 -columnspan 2 -sticky news
grid $pilot_interface.status_can -row 2 -column 0 -columnspan 2 -sticky ew
grid $pilot_interface.mode_frm   -row 3 -column 0 -sticky news
grid $pilot_interface.jog_frm    -row 4 -column 0 -columnspan 1 -sticky news
grid $pilot_interface.offset_frm -row 3 -column 1 -rowspan 2    -sticky news
grid $pilot_interface.move_frm   -row 5 -column 0 -columnspan 2 -sticky news

update_status_frame

}

proc update_status_frame {} {
    
#	response variables
#    set resp_move "NO"
#    set resp_mode "IDLE"
    if ![ winfo exists .pilot_interface ] { return }	
	
    # check the connection status to the controller
    set resp [get_auto_rov_connect]
    
    if { $resp > 0 } {
        
        # if you're connected set the status indicator
        set_con_stat green

        # get the current controller mode and update status
        set resp [send_auto_rov_cmd "GET MODE"]

        # if it's idle disable all of the move controls
        if { $resp == "IDLE" } {
                set_mid_stat black
                set_ben_stat black
                set_mov_stat black
                set_dvl_stat black

                disable_jog 1
                disable_offset
                disable_move

        } else {

            # if you're not IDLE enable the offset frame
            enable_offset

            # if it's midwater  or benthic 
            if { $resp == "MIDWATER" } {
                    set_mid_stat green
                    set_ben_stat black
                    set_dvl_stat green
            }

            if { $resp == "BENTHIC" } {
                    set_mid_stat black
                    set_ben_stat green
                    set_dvl_stat green
            }

            # see if you're moving and update status and frames
            set resp [send_auto_rov_cmd "GET MOVING"]

            if { $resp == "YES" } {
                set_mov_stat green
                disable_jog 0
                disable_move
            } else {
                set_mov_stat red
                enable_jog
                enable_move
            }

        }

    } else {
        
        # set the GUI state to disconnected if you lose connection
        gui_disconnect
    }

    
    # keep checking the controller status
    after 250 update_status_frame
}


##############################################################################
#                                                                            #
# udp package tests below, not using for now                                 #
#                                                                            #
##############################################################################

if { 0 } {

set junk_toggle 0

proc udpEventHandler {sock} {
    global junk_toggle pilot_interface
    global gyro_id
    global tms_id
    set pkt [read $sock]
    set peer [fconfigure $sock -peer]

    $pilot_interface.move_frm.entry_frm.dist_ent    delete 0 10
#    $pilot_interface.move_frm.entry_frm.bearing_ent delete 0 10
#    $pilot_interface.move_frm.entry_frm.rate_ent    delete 0 10

    $pilot_interface.move_frm.entry_frm.dist_ent    insert 0 $pkt
#    $pilot_interface.move_frm.entry_frm.bearing_ent insert 0 2.0
#    $pilot_interface.move_frm.entry_frm.rate_ent    insert 0 3.0


    if {$junk_toggle > 0} {
        set junk_toggle 0
        $pilot_interface.status_can delete $tms_id
        $pilot_interface.status_can delete $gyro_id
        set gyro_id [ $pilot_interface.status_can create rectangle 80 10 90 20 -fill red ]
        set tms_id [ $pilot_interface.status_can create rectangle 80 30 90 40 -fill green ]
    } else {
        set junk_toggle 1
        .status_can delete $tms_id
        .status_can delete $gyro_id
        set gyro_id [ $pilot_interface.status_can create rectangle 80 10 90 20 -fill green ]
        set tms_id [ $pilot_interface.status_can create rectangle 80 30 90 40 -fill red ]
    }
    puts "$peer: [string length $pkt] {$pkt}"
    return
}


proc udp_listen {port} {
    set srv [udp_open $port]
    fconfigure $srv -buffering none -translation binary
    fileevent $srv readable [list ::udpEventHandler $srv]
    puts "Listening on udp port: [fconfigure $srv -myport]"
    return $srv
}

}