##############################################################################
# Copyright 2003 MBARI.                                                      #
# Monterey Bay Aquarium Research Institute Proprietary Information.          #
# All rights reserved.                                                       #
##############################################################################

array set pos_offset { x {} y {} z {} }
array set prev_pos_offset [array get pos_offset]
array set pos_offset_store [array get pos_offset]
# set pos_offset_sent_lately false **********Dave edit 06/08*********
set pos_offset_sent_lately 0

array set default_slider_range { x,lo 2 x,hi -2 y,lo -2 y,hi 2 z,lo -1 z,hi 1 }
array set slider_range [array get default_slider_range]

# disable commands
proc disable_offset { } {
    global sliders pilot_interface
    
    $pilot_interface.offset_frm.zero_btn configure -state disabled
    $pilot_interface.offset_frm.jog_btn configure -state disabled

    $sliders(x) configure -state disabled
    $sliders(y) configure -state disabled
}

proc enable_offset { } {
    global sliders pilot_interface
    
    $pilot_interface.offset_frm.zero_btn configure -state normal
    $pilot_interface.offset_frm.jog_btn configure -state normal

    $sliders(x) configure -state normal
    $sliders(y) configure -state normal
}


# interval at which to send offset updates (to avoid flooding the network)
set offset_send_interval 1000

# interval at which to reenable slider
# to prevent the check_slider_rate from constantly updating the slider range 
# if it has been moved to its full extent with the mouse, the slider is 
# temporarily disabled when the range is updated
set slider_reenable_interval 1000

set small_inc(dist) 0.2
set big_inc(dist) 2.0

set small_inc(slew_rate) 0.01
set big_inc(slew_rate) 0.1

set inc_var dist

set rate_input $slew_rate
set dist_input $dist
set bearing_input 0.0
set prev_slew_rate $slew_rate
set prev_dist $dist

# commands
proc stored_value_exists {} {
    global pos_offset_store
    
    return [expr {$pos_offset_store(x) != {}}]
}

proc send_stored_value {} {
    global pos_offset_store pos_offset_sent_lately
    # only send commands if you're connected
    send_auto_rov_cmd "OFFSET $pos_offset_store(x) $pos_offset_store(y)"
        
    set pos_offset_sent_lately 1
    set pos_offset_store(x) {}
}
    
proc limit_offset_send_rate {} {
    global offset_send_interval pos_offset_sent_lately

    # there is a value to spit out
    if {[stored_value_exists]} { 
	send_stored_value
    } else {
	set pos_offset_sent_lately 0
    }

    # check again in a little while
    after $offset_send_interval limit_offset_send_rate
}

proc check_slider_range {axis val} {
    global sliders slider_range slider_reenable_interval

    set mov_incr [expr {($slider_range($axis,hi) - $slider_range($axis,lo))/2}]

    if {$val == $slider_range($axis,lo)} {
	set mov_incr [expr {-$mov_incr}]
	set at_range_limit true
    } elseif {$val == $slider_range($axis,hi)} {
	set at_range_limit true
    } else {
	set at_range_limit false
    }

    if {$at_range_limit} {
	incr slider_range($axis,lo) $mov_incr
	incr slider_range($axis,hi) $mov_incr
	
        $sliders($axis) configure -from $slider_range($axis,lo) -to \
            $slider_range($axis,hi) -state disabled
	
        after $slider_reenable_interval \
            [list $sliders($axis) configure -state normal]
    }
}
	
proc update_offsets {{val 0}} {
    global pos_offset prev_pos_offset
    global pos_offset_store pos_offset_sent_lately
 
    set update_pos false

    # check whether values have changed, as event gets triggered 
    foreach axis {x y z} {
	if {$pos_offset($axis) != $prev_pos_offset($axis)} {
	    check_slider_range $axis $pos_offset($axis)
	    set prev_pos_offset($axis) $pos_offset($axis)
	    set update_pos true
	}
    }
    if {$update_pos} {
	# store offset values for sending in rate-limited fashion
	foreach axis {x y z} {
	    set pos_offset_store($axis) $pos_offset($axis) 
	}
	if {!$pos_offset_sent_lately} {
	    # send immediately for somewhat faster control response
	    send_stored_value
	}
    }
}

set offset_slider_length 120
set offset_slider_width 30
set offset_slider_options [list \
			   -width $offset_slider_width \
			   -length  $offset_slider_length \
			   -showvalue true \
			       -command update_offsets ]
			

proc zero_position_offset {} {
    global pos_offset
    global sliders slider_range default_slider_range

    array set slider_range [array get default_slider_range]

    foreach axis {x y} {
	set pos_offset($axis) 0.0
	
        $sliders($axis) configure -from $slider_range($axis,lo) -to \
            $slider_range($axis,hi)
	
        set pos_offset($axis) 0.0
    }

    update_offsets
}

proc jog_to_position_offset {} {
    global pos_offset slew_rate
    
    # if both x and y sliders are zero, just return
    if { $pos_offset(x) == 0 && $pos_offset(y) == 0} {
        return
    }

    set pi [expr {acos(-1)}]
    set offset_dist [expr {sqrt($pos_offset(x)*$pos_offset(x) + \
        $pos_offset(y)*$pos_offset(y))}]
    set offset_ang  [expr {atan2($pos_offset(y),$pos_offset(x))*180./$pi}]
    
    # only send commands if you're connected
    send_auto_rov_cmd "MOVE $offset_ang, $offset_dist, $slew_rate"
    zero_position_offset
}

proc offset_slider {axis {orient vertical}} {
    global offset_slider_options pos_offset slider_range pilot_interface

    set r0 $slider_range($axis,lo)
    set r1 $slider_range($axis,hi)

    set range_params [list -from $r0 -to $r1]

    set span [expr {abs($r0 - $r1)}]
    set tick_params  [list -tickinterval \
        [expr {$span/4.0}] -resolution [expr {$span/40.0} ]]  

    set label "[string toupper $axis] offset"
    set cmd [list scale .pilot_interface.offset_frm.${axis}_scale \
    		 -variable pos_offset($axis) -orient $orient \
		 -label $label ]
    return [eval [concat $cmd $range_params $tick_params \
        $offset_slider_options ]]
}

# start up offset sending limiter
limit_offset_send_rate