#-------------------------------#
# Initialize the variables     	#
#-------------------------------#

# Row 1
set l1c1 925920
set l1c2 925920
set l1c3 15840
set l1c4 26400
# Row 2
set l2c1 1339560
set l2c2 1339560
set l2c3 78400
set l2c4 108900
# Row 3
set l3c1 0.5
set l3c2 0.5
set l3c3 0.5
set l3c4 0.5
# Row 4
set l4c1 0
set l4c2 0
set l4c3 0.25
set l4c4 0.25        	

set teleos_timer 0
set to_teleos1 "Tranmitted to Teleos Line 1"
set to_teleos2 "Tranmitted to Teleos Line 2"
set from_teleos1 "Received From Teleos Line 1"
set from_teleos2 "Received From Teleos Line 2"

set dvl_lable_data " XVbot, YVbot, ZVbot, EVbot,  Bm1,  Bm2,  Bm3,  Bm4,BS, XVref, YVref, ZVref, EVref,WS,Hr,Mn,Sc,HS,Temp,Pitch, Roll,  yaw"

set dvl_lable_data2 "DMG-Xbottom,DMG-Ybottom,DMG-Zbottom,DMG-Ebottom,DMG-Xwater ,DMG-Ywater ,DMG-Zwater ,DMG-Ewater "

#---------------------------------------------------------#
# default mode for vision system
#---------------------------------------------------------#

set teleos_mode 1

for {set i 1} {$i <= 4} {incr i} {

	set l1c$i 0
	set l2c$i 0
	set l3c$i 0
	set l4c$i 0
	
}

#-------------------------------------------------------#
# This procedure create a window that enable the user  	#
# to enter constants to set up the teleos communication	#
#-------------------------------------------------------#

proc teleos_setup { } {
    global teleos_timer
    global l1c1 l1c2 l1c3 l1c4 l2c1 l2c2 l2c3 l2c4
    global l3c1 l3c2 l3c3 l3c4 l4c1 l4c2 l4c3 l4c4         	
    global teleos teleos_mode
    global to_teleos1 to_teleos2 from_teleos1 from_teleos2
    global dvl_data dvl_lable_data dvl_data2 dvl_lable_data2
    global teleos_ethernet_freq_rx teleos_ethernet_freq_tx
    set teleos .teleos_setup

    global env
    set env(DISPLAY) :0
    toplevel $teleos -width 27c -height 17c
    wm title $teleos "Teleos Setup"

    #-------------------------------------------#
    # Horizontal label on the top		#
    #-------------------------------------------#

    label $teleos.lab52 \
        -text "Foreaft"
    label $teleos.lab53 \
        -text "Turns"
    label $teleos.lab54 \
        -text "Lateral"
    label $teleos.lab55 \
        -text "Vertical"

    #-------------------------------------------#
    # Vertical label on the left hand side	#
    #-------------------------------------------#

    label $teleos.lab56 \
        -text "KF1"
    label $teleos.lab57 \
        -text "KF2"
    label $teleos.lab58 \
        -text "DB"
    label $teleos.lab59 \
        -text "ST"


    #-------------------------------------------#
    # Column 1                               	#
    #-------------------------------------------#
    entry $teleos.ent60 \
        -background white -textvariable l1c1 -width 78
    entry $teleos.ent61 \
        -background white -textvariable l2c1 -width 78
    entry $teleos.ent62 \
        -background white -textvariable l3c1 -width 78
    entry $teleos.ent63 \
        -background white -textvariable l4c1 -width 78


    #-------------------------------------------#
    # Column 2                               	#
    #-------------------------------------------#
    entry $teleos.ent64 \
        -background white -textvariable l1c2 -width 78
    entry $teleos.ent65 \
        -background white -textvariable l2c2 -width 78
    entry $teleos.ent66 \
        -background white -textvariable l3c2 -width 78
    entry $teleos.ent67 \
        -background white -textvariable l4c2 -width 78

    #-------------------------------------------#
    # Column 3                               	#
    #-------------------------------------------#
    entry $teleos.ent68 \
        -background white -textvariable l1c3 -width 78
    entry $teleos.ent69 \
        -background white -textvariable l2c3 -width 78
    entry $teleos.ent70 \
        -background white -textvariable l3c3 -width 78
    entry $teleos.ent79 \
        -background white -textvariable l4c3 -width 78

    #-------------------------------------------#
    # Column 4                               	#
    #-------------------------------------------#
    entry $teleos.ent80 \
        -background white -textvariable l1c4 -width 83
    entry $teleos.ent81 \
        -background white -textvariable l2c4 -width 83
    entry $teleos.ent82 \
        -background white -textvariable l3c4 -width 83
    entry $teleos.ent83 \
        -background white -textvariable l4c4 -width 83

    #-------------------------------------------#
    # Title					#
    #-------------------------------------------#
    label $teleos.lab86 \
        -foreground blue -relief groove -text {Teleos Setup} 


    #-------------------------------------------#
    # The string to and from Teleos		#
    #-------------------------------------------#
    label $teleos.lab_to_teleos \
        -text "TX"
    label $teleos.lab_from_teleos \
        -text "RX"
    entry $teleos.ent_to_teleos1 \
        -background #d9d9d9 -textvariable to_teleos1 -state disabled
    entry $teleos.ent_from_teleos1 \
        -background #d9d9d9 -textvariable from_teleos1 -state disabled
    entry $teleos.ent_to_teleos2 \
        -background #d9d9d9 -textvariable to_teleos2 -state disabled
    entry $teleos.ent_from_teleos2 \
        -background #d9d9d9 -textvariable from_teleos2 -state disabled

    #-------------------------------------------#
    # The dvl string to Teleos		        #
    #-------------------------------------------#
    label $teleos.lab_dvl1_teleos \
        -text "DVL"
    label $teleos.lab_dvl2_teleos \
        -text "DVL" 
    entry $teleos.ent_dvl1_teleos \
        -background #d9d9d9 -textvariable dvl_lable_data -state disabled -font {Halvetica 10}
    entry $teleos.ent_dvl2_teleos \
        -background #d9d9d9 -textvariable dvl_data -state disabled -font {Halvetica 10}
    label $teleos.lab_dvl3_teleos \
        -text "DVL"
    label $teleos.lab_dvl4_teleos \
        -text "DVL" 
    entry $teleos.ent_dvl3_teleos \
        -background #d9d9d9 -textvariable dvl_lable_data2 -state disabled -font {Halvetica 10}
    entry $teleos.ent_dvl4_teleos \
        -background #d9d9d9 -textvariable dvl_data2 -state disabled -font {Halvetica 10}


    #-------------------------------------------#
    # The Ethernet timeing Teleos		        #
    #-------------------------------------------#
    label $teleos.lab_ethernet_tx_teleos \
        -text "Ethernet Transmit (Hz)"
    label $teleos.lab_ethernet_rx_teleos \
        -text "Ethernet Receive (Hz)"
    entry $teleos.ent_ethernet_tx_teleos \
        -background #d9d9d9 -textvariable teleos_ethernet_freq_tx -state disabled
    entry $teleos.ent_ethernet_rx_teleos \
        -background #d9d9d9 -textvariable teleos_ethernet_freq_rx -state disabled


    #-------------------------------------------#
    # Mode					#
    #-------------------------------------------#

    label $teleos.lab85 \
        -text Mode

    button $teleos.but87 \
        -height 21 -text v -width 27

    set td1 1
    set td2 2
    set td3 3

    radiobutton $teleos.r1 \
        -text "Old Mode" -variable teleos_mode -value $td1  \
        -command { setting_up_teleos } -font {Halvetica 10}
    radiobutton $teleos.r2 \
        -text "Pass Through" -variable teleos_mode -value $td2 \
        -command { setting_up_teleos } -font {Halvetica 10}
    radiobutton $teleos.r3 \
        -text "Reverse Mode" -variable teleos_mode -value $td3  \
        -command { setting_up_teleos } -font {Halvetica 10}

    #-------------------------------------------#
    # Ok, Close Button				#
    #-------------------------------------------#

    button $teleos.but88 \
        -command { setting_up_teleos } -height 46 -text Ok -width 62
    button $teleos.but89 \
        -command { set teleos_timer 0; destroy .teleos_setup } -height 46 -text Close -width 62

    #############################
    # SETTING GEOMETRY 		#
    #############################

    place $teleos.lab52 \
        -x 100 -y 70 -anchor nw -bordermode ignore
    place $teleos.lab53 \
        -x 190 -y 70 -anchor nw -bordermode ignore
    place $teleos.lab54 \
        -x 275 -y 70 -anchor nw -bordermode ignore
    place $teleos.lab55 \
        -x 360 -y 70 -anchor nw -bordermode ignore
    place $teleos.lab56 \
        -x 35 -y 95 -anchor nw -bordermode ignore
    place $teleos.lab57 \
        -x 35 -y 120 -anchor nw -bordermode ignore
    place $teleos.lab58 \
        -x 35 -y 145 -anchor nw -bordermode ignore
    place $teleos.lab59 \
        -x 35 -y 170 -anchor nw -bordermode ignore
    place $teleos.ent60 \
        -x 85 -y 95 -width 78 -height 22 -anchor nw -bordermode ignore
    place $teleos.ent61 \
        -x 85 -y 120 -width 78 -height 22 -anchor nw -bordermode ignore
    place $teleos.ent62 \
        -x 85 -y 145 -width 78 -height 22 -anchor nw -bordermode ignore
    place $teleos.ent63 \
        -x 85 -y 170 -width 78 -height 22 -anchor nw -bordermode ignore
    place $teleos.ent64 \
        -x 170 -y 95 -width 78 -height 22 -anchor nw -bordermode ignore
    place $teleos.ent65 \
        -x 170 -y 120 -width 78 -height 22 -anchor nw -bordermode ignore
    place $teleos.ent66 \
        -x 170 -y 145 -width 78 -height 22 -anchor nw -bordermode ignore
    place $teleos.ent67 \
        -x 170 -y 170 -width 78 -height 22 -anchor nw -bordermode ignore
    place $teleos.ent68 \
        -x 255 -y 95 -width 78 -height 22 -anchor nw -bordermode ignore
    place $teleos.ent69 \
        -x 255 -y 120 -width 78 -height 22 -anchor nw -bordermode ignore
    place $teleos.ent70 \
        -x 255 -y 145 -width 78 -height 22 -anchor nw -bordermode ignore
    place $teleos.ent79 \
        -x 255 -y 170 -width 78 -height 22 -anchor nw -bordermode ignore
    place $teleos.ent80 \
        -x 340 -y 95 -width 83 -height 22 -anchor nw -bordermode ignore
    place $teleos.ent81 \
        -x 340 -y 120 -width 83 -height 22 -anchor nw -bordermode ignore
    place $teleos.ent82 \
        -x 340 -y 145 -width 83 -height 22 -anchor nw -bordermode ignore
    place $teleos.ent83 \
        -x 340 -y 170 -width 83 -height 22 -anchor nw -bordermode ignore
    place $teleos.lab86 \
        -x 35 -y 10 -width 689 -height 20 -anchor nw -bordermode ignore

    #-------------------------------------------#
    # Teleos strings 				#
    #-------------------------------------------#
    place $teleos.lab_to_teleos \
        -x 5 -y 275 -anchor nw -bordermode ignore
    place $teleos.lab_from_teleos \
        -x 5 -y 325 -anchor nw -bordermode ignore
    place $teleos.ent_to_teleos1 \
        -x 25 -y 275 -width 700 -height 22 -bordermode ignore
    place $teleos.ent_to_teleos2 \
        -x 25 -y 300 -width 700 -height 22 -bordermode ignore
    place $teleos.ent_from_teleos1 \
        -x 25 -y 325 -width 700 -height 22 -bordermode ignore
    place $teleos.ent_from_teleos2 \
        -x 25 -y 350 -width 700 -height 22 -bordermode ignore

    #-------------------------------------------#
    # DVL string 				#
    #-------------------------------------------#
    place $teleos.lab_dvl1_teleos \
        -x 2 -y 375 -anchor nw -bordermode ignore
    place $teleos.lab_dvl2_teleos \
        -x 2 -y 400 -anchor nw -bordermode ignore
    place $teleos.ent_dvl1_teleos \
        -x 30 -y 375 -width 730 -height 22 -bordermode ignore
    place $teleos.ent_dvl2_teleos \
        -x 30 -y 400 -width 730 -height 22 -bordermode ignore
    place $teleos.lab_dvl3_teleos \
        -x 2 -y 425 -anchor nw -bordermode ignore
    place $teleos.lab_dvl4_teleos \
        -x 2 -y 450 -anchor nw -bordermode ignore
    place $teleos.ent_dvl3_teleos \
        -x 30 -y 425 -width 730 -height 22 -bordermode ignore
    place $teleos.ent_dvl4_teleos \
        -x 30 -y 450 -width 730 -height 22 -bordermode ignore

    #-------------------------------------------#
    # Ethernet Timing 			 				#
    #-------------------------------------------#
    place $teleos.lab_ethernet_tx_teleos \
        -x 450 -y 100 -anchor nw -bordermode ignore
    place $teleos.lab_ethernet_rx_teleos \
        -x 450 -y 125 -anchor nw -bordermode ignore
    place $teleos.ent_ethernet_tx_teleos \
        -x 600 -y 100 -width 50 -height 22 -bordermode ignore
    place $teleos.ent_ethernet_rx_teleos \
        -x 600 -y 125 -width 50 -height 22 -bordermode ignore

    #-------------------------------------------#
    # Mode ( Radiobutton )			#
    #-------------------------------------------#

    place $teleos.lab85 \
        -x 35 -y 40 -anchor nw -bordermode ignore
    place $teleos.r1 \
        -x 100 -y 40 -anchor nw -bordermode ignore
    place $teleos.r2 \
        -x 180 -y 40 -anchor nw -bordermode ignore
    place $teleos.r3 \
        -x 280 -y 40 -anchor nw -bordermode ignore


    #-------------------------------------------#
    # Ok, Close Button				#
    #-------------------------------------------#

    place $teleos.but88 \
        -x 290 -y 210 -width 62 -height 46 -anchor nw -bordermode ignore
    place $teleos.but89 \
        -x 360 -y 210 -width 62 -height 46 -anchor nw -bordermode ignore 


    #-------------------------------------------#
    # Obtain data from database			#
    #-------------------------------------------#
    set teleos_string [get_teleos_info]

    for {set i 1} {$i <= 4} {incr i} {
    	
    	set j [expr {$i - 1} ]
    	
	set l1c$i [lindex $teleos_string $j]
	set l2c$i [lindex $teleos_string [expr {$j + 4} ] ]
	set l3c$i [lindex $teleos_string [expr {$j + 8} ] ]
	set l4c$i [lindex $teleos_string [expr {$j + 12} ] ]
	
    }

    set teleos_mode [lindex $teleos_string 16 ]

    set tel_var "$l1c1 $l1c2 $l1c3 $l1c4 $l2c1 $l2c2 $l2c3 $l2c4 $l3c1 $l3c2 $l3c3 $l3c4 $l4c1 $l4c2 $l4c3 $l4c4 $teleos_mode"
    puts "teleos_setup.tcl: tel_var : $tel_var"                                                 	
	
    set to_teleos1 [get_teleos_tx_string 0 ]
    set to_teleos2 [get_teleos_tx_string 1 ]

    set from_teleos1 [get_teleos_rx_string 0 ]
    set from_teleos2 [get_teleos_rx_string 1 ]
    set dvl_data [get_dvl_data 1]
    set dvl_data2 [get_dvl_data 2]
    
    # Loop to update the master and feedback values
    set teleos_timer 1
    loop_to_from_transmission
}



proc setting_up_teleos { } {

	global teleos_mode
       	global l1c1 l1c2 l1c3 l1c4 l2c1 l2c2 l2c3 l2c4
       	global l3c1 l3c2 l3c3 l3c4 l4c1 l4c2 l4c3 l4c4
       	
	
	#-----------------------------------------------#
	# Send the information down to database		#
	# Setup data goes to "vehicle.cpp"		#
	# Also, adjust teleos thrusters curve to be	#
	#-----------------------------------------------#

	set tel_var "$l1c1 $l1c2 $l1c3 $l1c4 $l2c1 $l2c2 $l2c3 $l2c4 $l3c1 $l3c2 $l3c3 $l3c4 $l4c1 $l4c2 $l4c3 $l4c4 $teleos_mode"
	puts "teleos_setup.tcl: tel_var : $tel_var"                                                 	
	
	set_teleos_setup $l1c1 $l1c2 $l1c3 $l1c4 $l2c1 $l2c2 $l2c3 $l2c4 $l3c1 $l3c2 $l3c3 $l3c4 $l4c1 $l4c2 $l4c3 $l4c4 $teleos_mode

	
}


 proc loop_to_from_transmission { } {

	global teleos_timer teleos
	global to_teleos1 to_teleos2 from_teleos1 from_teleos2
	global dvl_data dvl_data2
	
	if { ![winfo exists $teleos] || $teleos_timer == 0} {return}
	
	set to_teleos1 [get_teleos_tx_string 0 ]
	set to_teleos2 [get_teleos_tx_string 1 ]
     	set from_teleos1 [get_teleos_rx_string 0 ]
     	set from_teleos2 [get_teleos_rx_string 1 ]
     	set dvl_data [get_dvl_data 1]
     	set dvl_data2 [get_dvl_data 2]	
     		
	after 100 [list loop_to_from_transmission]
}