#!/usr/local/bin/wish8.2

#### CHANGE ABOVE FOR YOUR TK INTERPRETER ! Version >= 8.2 !!!!!





##################################################################
########      FUNCTION      ######################################
##################################################################

# this function set autorepeat interval for arrow buttons

proc ChangeAutorepeat { tm } {
    global panel
    if { $panel == 1 } {
    	for { set i 1 } { $i < 7 } { incr i } {   
	    .main.tools.scrolls.s$i configure -repeatinterval $tm 
	}  
    }
}



##################################################################
########      FUNCTION      ######################################
##################################################################

# this function sends new set of speed limits to robot server

proc SetSpeeds { } {

    global speed
    global bg_speed
    global sg_speed
    
    set command $::SET_SPEEDS,1
    for { set i 0 } { $i < 6 } { incr i } {
	set pspeed [ expr $speed($i) / 57.296 ]
	set command $command,$pspeed
    }
    set command $command,$bg_speed,$sg_speed
    SendCommand $command 
    
    AskServerAboutSpeeds
}


##################################################################
########      FUNCTION      ######################################
##################################################################

# this function opens menu for changing / quering robot speeds

proc Speeds { } {

    global speed
    global bg_speed
    global sg_speed

    if { [ winfo exists .speeds ] != 1 } {
	
	toplevel .speeds -borderwidth 10 -background blue \
	    -relief raised
	wm title .speeds "Speeds"
	wm minsize .speeds 220 370
	
	label .speeds.l -background lightblue -relief groove \
	    -text " Joints and grippers speeds" \
	    -borderwidth 5
	pack  .speeds.l -side top -pady 5 -padx 3

	frame .speeds.sp -background blue 
	pack .speeds.sp -side top -pady 3 -padx 3

	frame .speeds.sp.joints -background blue -relief raised \
	    -borderwidth 2
	pack .speeds.sp.joints -side left -padx 3 -pady 3

	label .speeds.sp.joints.ll -text " Joints, deg/sec" \
	    -background blue -height 2 \
	    -foreground yellow
	pack .speeds.sp.joints.ll -side top -pady 2

	frame .speeds.sp.joints.l -background lightblue \
	    -relief groove -borderwidth 2
	pack .speeds.sp.joints.l -side left -padx 3 -pady 3

	frame .speeds.sp.joints.v -background blue \
	    -relief groove -borderwidth 2
	pack .speeds.sp.joints.v -side left -padx 3 -pady 3

	for { set i 1 } { $i < 7 } { incr i } {
	    set x $i
	    incr x -1
	    label .speeds.sp.joints.l.l$i -text  " $i " \
		-background lightblue
	    pack  .speeds.sp.joints.l.l$i -side top
	    entry .speeds.sp.joints.v.v$i \
		-relief sunken -width 6 \
		-textvariable speed($x)
	    pack  .speeds.sp.joints.v.v$i -side top
	}

	frame .speeds.sp.r -background blue
	pack .speeds.sp.r -side right -padx 3 -pady 3

	frame .speeds.sp.r.grippers -background blue \
	    -relief raised -borderwidth 2
	pack .speeds.sp.r.grippers -side top -padx 3 -pady 3

	label .speeds.sp.r.grippers.ll -height 2 \
	    -text "Grippers,\n mm/sec" \
	    -background blue -foreground yellow 
	pack .speeds.sp.r.grippers.ll -side top -padx 5 -pady 5

	label .speeds.sp.r.grippers.lbig -text "Big" -width 6 \
	    -background lightblue -relief groove -borderwidth 2
	pack .speeds.sp.r.grippers.lbig -side top -padx 2 -pady 2

	entry .speeds.sp.r.grippers.vbig -width 5 \
	    -borderwidth 2 -textvariable bg_speed
	pack .speeds.sp.r.grippers.vbig -side top -padx 2 -pady 3
	
	label .speeds.sp.r.grippers.lsmall -text "Small" -width 6 \
	    -background lightblue -relief groove -borderwidth 2
	pack .speeds.sp.r.grippers.lsmall -side top -padx 2 -pady 2

	entry .speeds.sp.r.grippers.vsmall -width 5 \
	    -borderwidth 2 -textvariable sg_speed
	pack .speeds.sp.r.grippers.vsmall -side top \
	    -padx 2 -pady 2

	button .speeds.sp.r.apply -text " Apply " -background red2 \
	    -activebackground red -command SetSpeeds \
	    -borderwidth 3
	pack .speeds.sp.r.apply -side bottom -pady 4

	frame .speeds.buttons -background blue \
	    -relief sunken -borderwidth 5
	pack  .speeds.buttons -side bottom -pady 3

	button .speeds.ask -borderwidth 5 \
	    -text " Ask server about speeds " \
	    -background yellow2 -activebackground yellow \
	    -command AskServerAboutSpeeds
	pack .speeds.ask -side top -padx 5 -pady 5

	button .speeds.buttons.quit -text " Close " \
	    -borderwidth 5 -background green2 \
	    -activebackground green \
	    -command { destroy .speeds }
	pack .speeds.buttons.quit -side top -padx 20 -pady 5

    }   
}


##################################################################
########      FUNCTION      ######################################
##################################################################

# this function asks robot server about current speeds' limits

proc AskServerAboutSpeeds { } {

    global netsocket
    global connected
    global speed
    global bg_speed
    global sg_speed

    . configure -cursor watch
    if { [winfo exists .speeds ] == 1 } { 
	.speeds configure -cursor watch 
    }
    update

    set command $::GET_INFO,$::SPEEDS_INFO
    set len [ binary format "c" [ string length $command ] ]

    if { $connected == 0 } {
	SetConnection
	if { $connected == 1 } {
	    puts -nonewline $netsocket $len 
	    puts -nonewline $netsocket $command  
	    flush $netsocket
	    set num \x00
	    catch { set num [ read $netsocket 1 ] } dummyerror
	    binary scan $num "c" len
	    if { $len < 0 } { set len [ expr  $len + 256 ] }
	    if { $len != 0 } {
		set reply [ read $netsocket $len ]
		
		if { [ scan $reply \
			   "%d %f %f %f %f %f %f %f %f" \
			   dummyvar \
			   speed(0) speed(1) speed(2) \
			   speed(3) speed(4) speed(5) \
			   bg_speed sg_speed ] == 9 } {
		    for { set i 0 } { $i < 6 } { incr i } { 
			set speed($i) [ expr { 57.3 * $speed($i) } ]
			set speed($i) [ format "%3.1f" $speed($i) ]
		    }
		}
	    }
	    close  $netsocket
	}
	set connected 0 
    }
    . configure -cursor left_ptr
    if { [winfo exists .speeds ] == 1 } { 
	.speeds configure -cursor left_ptr 
    }
    update  
}


##################################################################
########      FUNCTION      ######################################
##################################################################

# this function sends new set of joints' and grippers' limits to 
# robot server 

proc SetLimits { } {

    global qmin
    global qmax
    global bg_min
    global bg_max
    global sg_min
    global sg_max

    set command $::SET_LIMITS,1
    for { set i 0 } { $i < 6 } { incr i } {
	set pmin [ expr $qmin($i) / 57.296 ]
	set pmax [ expr $qmax($i) / 57.296 ]
	set command $command,$pmin,$pmax
    }
    set command  $command,$bg_min,$bg_max,$sg_min,$sg_max
    SendCommand $command 
    AskServerAboutLimits
    
}


##################################################################
########      FUNCTION      ######################################
##################################################################

# this function sends command to remove specified object

proc RemoveObject { num } {

    SendCommand $::REMOVE_OBJECT,$num  
}

##################################################################
########      FUNCTION      ######################################
##################################################################

# this function asks server about current joint's and grippers' limits

proc AskServerAboutLimits { } {

    global netsocket
    global connected
    global qmin
    global qmax
    global bg_min
    global bg_max
    global sg_min
    global sg_max
    global cpu
    global net


    . configure -cursor watch
    if { [winfo exists .limits ] == 1 } { 
	.limits configure -cursor watch 
    }
    update

    set command $::GET_INFO,$::LIMITS_INFO
    set len [ binary format "c" [ string length $command ] ]

    if { $connected == 0 } {
	SetConnection
	if { $connected == 1 } {
	    puts -nonewline $netsocket $len 
	    puts -nonewline $netsocket $command  
	    flush $netsocket
	    set num \x00
	    catch { set num [ read $netsocket 1 ] } dummyerror
	    binary scan $num "c" len
	    if { $len < 0 } { set len [ expr  $len + 256 ] }
	    if { $len != 0 } {
		set reply [ read $netsocket $len ]
		
		if { [ scan $reply \
       "%d %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f" \
			   dummyvar \
			   qmin(0) qmax(0) qmin(1) qmax(1) \
			   qmin(2) qmax(2) qmin(3) qmax(3) \
			   qmin(4) qmax(4) qmin(5) qmax(5) \
			   bg_min bg_max sg_min sg_max \
			   cpu net ] == 19
		 } {
		    for { set i 0 } { $i < 6 } { incr i } { 
			set qmin($i) [ expr { 57.296 * $qmin($i) } ]
			set qmax($i) [ expr { 57.296 * $qmax($i) } ]
			set qmin($i) [ format "%3.1f" $qmin($i) ]
			set qmax($i) [ format "%3.1f" $qmax($i) ]
		    }
		    CorrectGripper
		    CorrectScales
		}
	    }
	    close  $netsocket
	}
	set connected 0 
    }
    . configure -cursor left_ptr
    if { [winfo exists .limits ] == 1 } { 
	.limits configure -cursor left_ptr 
    }
    update
}



##################################################################
########      FUNCTION      ######################################
##################################################################

# this function sends command to add new object into simulation scene

proc AddObject { } {

    global sizex
    global sizey
    global sizez
    global centerx
    global centery
    global centerz
    global redv
    global greenv
    global bluev
    global F

    set rgbcolor [ expr { $redv * 10000 } ]  
    set mult [ expr { $greenv * 100 } ]  
    set rgbcolor [ expr { $rgbcolor + $mult } ]  
    set prm [ expr { $rgbcolor + $bluev } ]  

    set prm $prm,$sizex,$sizey,$sizez
    set prm $prm,$centerx,$centery,$centerz
    for { set i 0 } { $i < 12 } { incr i } {
	set prm $prm,$F($i)
    }

    SendCommand $::ADD_OBJECT,$prm
}


##################################################################
########      FUNCTION      ######################################
##################################################################

# this function opens menu for adding / removing objects in 
# simulation scene

proc Objects { } {

    global sizex
    global sizey
    global sizez
    global centerx
    global centery
    global centerz
    global redv
    global greenv
    global bluev

    global F


    if { [ winfo exists .objects ] != 1 } {
	
	toplevel .objects -borderwidth 10 -background blue \
	    -relief raised
	wm title .objects "Objects"
	wm minsize .objects 300 410
	
	label .objects.l -background lightblue -relief groove \
	    -text " Object management " \
	    -borderwidth 5
	pack  .objects.l -side top -pady 5 -padx 5

	frame .objects.add -background blue \
	    -relief groove -borderwidth 3
	pack  .objects.add -side top -pady 3 -padx 2
	
	frame .objects.add.top -background blue
	pack  .objects.add.top -side top -padx 2
	
	# sizes of the object
	frame .objects.add.top.s -background blue -relief raised \
	    -borderwidth 2
	pack  .objects.add.top.s -side left -pady 2 -padx 2
	label .objects.add.top.s.l -text " Sizes, mm " \
	    -relief groove -background lightblue
	pack  .objects.add.top.s.l -side top -pady 2 -padx 2
	
	frame .objects.add.top.s.s1 -background blue
	pack  .objects.add.top.s.s1 -side top
	label .objects.add.top.s.s1.l -background blue \
	    -text x -width 2 -foreground yellow
	pack .objects.add.top.s.s1.l -side left -pady 3 -padx 2
	entry .objects.add.top.s.s1.e -width 5 -textvariable sizex
	pack .objects.add.top.s.s1.e -side right -pady 2

	frame .objects.add.top.s.s2 -background blue
	pack  .objects.add.top.s.s2 -side top
	label .objects.add.top.s.s2.l -background blue \
	    -text y -width 2 -foreground yellow
	pack .objects.add.top.s.s2.l -side left -pady 3 -padx 2
	entry .objects.add.top.s.s2.e -width 5 -textvariable sizey
	pack .objects.add.top.s.s2.e -side right -pady 2

	frame .objects.add.top.s.s3 -background blue
	pack  .objects.add.top.s.s3 -side top
	label .objects.add.top.s.s3.l -background blue \
	    -text z -width 2 -foreground yellow
	pack .objects.add.top.s.s3.l -side left -pady 3 -padx 2 
	entry .objects.add.top.s.s3.e -width 5 -textvariable sizez
	pack .objects.add.top.s.s3.e -side right -pady 2



	# center of the object
	frame .objects.add.top.cn -background blue -relief raised \
	    -borderwidth 2 
	pack  .objects.add.top.cn -side left -pady 2 -padx 2
	label .objects.add.top.cn.l -text " Center, mm " \
	    -relief groove -background lightblue
	pack  .objects.add.top.cn.l -side top -pady 2 -padx 2

	frame .objects.add.top.cn.s1 -background blue
	pack  .objects.add.top.cn.s1 -side top
	label .objects.add.top.cn.s1.l -background blue \
	    -text x -width 2 -foreground yellow
	pack .objects.add.top.cn.s1.l -side left -pady 3 -padx 2
	entry .objects.add.top.cn.s1.e -width 5 -textvariable centerx
	pack .objects.add.top.cn.s1.e -side right -pady 2

	frame .objects.add.top.cn.s2 -background blue
	pack  .objects.add.top.cn.s2 -side top
	label .objects.add.top.cn.s2.l -background blue \
	    -text y -width 2 -foreground yellow
	pack .objects.add.top.cn.s2.l -side left -pady 3 -padx 2
	entry .objects.add.top.cn.s2.e -width 5 -textvariable centery
	pack .objects.add.top.cn.s2.e -side right -pady 2

	frame .objects.add.top.cn.s3 -background blue
	pack  .objects.add.top.cn.s3 -side top
	label .objects.add.top.cn.s3.l -background blue \
	    -text z -width 2 -foreground yellow
	pack .objects.add.top.cn.s3.l -side left -pady 3 -padx 2 
	entry .objects.add.top.cn.s3.e -width 5 -textvariable centerz
	pack .objects.add.top.cn.s3.e -side right -pady 2



	# color of the object
	frame .objects.add.top.co -background blue -relief raised \
	    -borderwidth 2 
	pack  .objects.add.top.co -side right -pady 2 -padx 2
	label .objects.add.top.co.l -text " Colors, % " \
	    -relief groove -background lightblue
	pack  .objects.add.top.co.l -side top -pady 2 -padx 2

	frame .objects.add.top.co.s1 -background blue
	pack  .objects.add.top.co.s1 -side top
	label .objects.add.top.co.s1.l -background blue \
	    -text red -width 4 -foreground yellow
	pack .objects.add.top.co.s1.l -side left -pady 3 -padx 2
	entry .objects.add.top.co.s1.e -width 2 -textvariable redv
	pack .objects.add.top.co.s1.e -side right -pady 2

	frame .objects.add.top.co.s2 -background blue
	pack  .objects.add.top.co.s2 -side top
	label .objects.add.top.co.s2.l -background blue \
	    -text green -width 4 -foreground yellow
	pack .objects.add.top.co.s2.l -side left -pady 3 -padx 2
	entry .objects.add.top.co.s2.e -width 2 -textvariable greenv
	pack .objects.add.top.co.s2.e -side right -pady 2

	frame .objects.add.top.co.s3 -background blue
	pack  .objects.add.top.co.s3 -side top
	label .objects.add.top.co.s3.l -background blue \
	    -text blue -width 4 -foreground yellow
	pack .objects.add.top.co.s3.l -side left -pady 3 -padx 2 
	entry .objects.add.top.co.s3.e -width 2 -textvariable bluev
	pack .objects.add.top.co.s3.e -side right -pady 2



	frame .objects.add.bottom -background blue
	pack  .objects.add.bottom -side bottom -padx 2

	# position
	frame .objects.add.bottom.f -background blue \
	    -relief raised -borderwidth 2
	pack  .objects.add.bottom.f -side left -pady 2 -padx 2
	label .objects.add.bottom.f.l -text " Position " \
	    -background lightblue -relief groove 
	pack  .objects.add.bottom.f.l -side top -pady 3

	set num 0
	for { set r 0 } { $r < 4 } { incr r } {

	    frame .objects.add.bottom.f.r$r -background blue
	    pack  .objects.add.bottom.f.r$r -side top -pady 1 -padx 1
	    for { set c 0 } { $c < 4 } { incr c } {
		entry .objects.add.bottom.f.r$r.e$c -width 4 \
		    -textvariable F($num)
		pack  .objects.add.bottom.f.r$r.e$c -side left \
		    -padx 2
		incr num
	    }
	}

	.objects.add.bottom.f.r3.e0 configure -state disabled 
	.objects.add.bottom.f.r3.e1 configure -state disabled
	.objects.add.bottom.f.r3.e2 configure -state disabled
	.objects.add.bottom.f.r3.e3 configure -state disabled


	button .objects.add.bottom.b -background green2 \
	    -borderwidth 5 -activebackground green \
	    -text " Add " -width 8 -height 4 -command AddObject
	pack  .objects.add.bottom.b -side right -pady 10 -padx 4

	frame .objects.remove -background blue -relief groove \
	    -borderwidth 3
	pack  .objects.remove -side top -pady 3 -padx 2


	menubutton .objects.remove.b -text " Remove object " \
	    -background red2 -activebackground red -width 25 \
	    -relief raised -borderwidth 3 -menu .objects.remove.b.m
	pack .objects.remove.b -side top -pady 4 -padx 37

	menu .objects.remove.b.m -background yellow \
	    -activebackground red

	for { set i 0 } { $i < 10 } { incr i } {
	    .objects.remove.b.m add command -label " N$i " \
		-command " RemoveObject $i "
	}

	frame .objects.quit -background blue \
	    -relief sunken -borderwidth 5
	pack  .objects.quit -side bottom -pady 3

	button .objects.quit.b -text " Close " \
	    -borderwidth 5 -background green2 \
	    -activebackground green \
	    -command { destroy .objects }
	pack .objects.quit.b -side bottom -padx 50 -pady 3
    }

}


##################################################################
########      FUNCTION      ######################################
##################################################################

# this function opens menu to configure / query current joints'
# and grippers' limits 

proc Limits { } {

    global qmin
    global qmax
    global bg_min
    global sg_min
    global bg_max
    global sg_max


    if { [ winfo exists .limits ] != 1 } {
	
	toplevel .limits -borderwidth 10 -background blue \
	    -relief raised
	wm title .limits "Limits"
	wm minsize .limits 320 315
	
	label .limits.l -background lightblue -relief groove \
	    -text " Joints and Grippers Limits " \
	    -borderwidth 5
	pack  .limits.l -side top -pady 5 -padx 3

	frame .limits.lim -background blue 
	pack .limits.lim -side top -pady 3 -padx 3

	frame .limits.lim.joints -background blue -relief raised \
	    -borderwidth 2
	pack .limits.lim.joints -side left -padx 3 -pady 3

	label .limits.lim.joints.ll -text " Joints, min-max,\n deg" \
	    -background blue -height 2 \
	    -foreground yellow
	pack .limits.lim.joints.ll -side top -pady 2

	frame .limits.lim.joints.l -background lightblue \
	    -relief groove -borderwidth 2
	pack .limits.lim.joints.l -side left -padx 3 -pady 3

	frame .limits.lim.joints.vmin -background blue \
	    -relief groove -borderwidth 2
	pack .limits.lim.joints.vmin -side left -padx 3 -pady 3

	frame .limits.lim.joints.vl -background blue
	pack .limits.lim.joints.vl -side left -pady 3

	frame .limits.lim.joints.vmax -background blue \
	    -relief groove -borderwidth 2
	pack .limits.lim.joints.vmax -side right -padx 3 -pady 3

	for { set i 1 } { $i < 7 } { incr i } {
	    set x $i
	    incr x -1
	    label .limits.lim.joints.l.l$i -text  " $i " \
		-background lightblue
	    pack  .limits.lim.joints.l.l$i -side top
	    entry .limits.lim.joints.vmin.v$i \
		-relief sunken -width 6 \
		-textvariable qmin($x)
	    pack  .limits.lim.joints.vmin.v$i -side top
	    
	    label .limits.lim.joints.vl.l$i \
		-text "-" -background blue -foreground yellow
	    pack  .limits.lim.joints.vl.l$i -side top

	    entry .limits.lim.joints.vmax.v$i \
		-relief sunken -width 6 \
		-textvariable qmax($x)
	    pack  .limits.lim.joints.vmax.v$i -side top
 
	}

	frame .limits.lim.r -background blue
	pack .limits.lim.r -side right -padx 3 -pady 3


	frame .limits.lim.r.grippers -background blue \
	    -relief raised -borderwidth 2
	pack .limits.lim.r.grippers -side top -padx 3 -pady 3

	label .limits.lim.r.grippers.ll -height 2 \
	    -text "Grippers,\n min-max, mm" \
	    -background blue -foreground yellow 
	pack .limits.lim.r.grippers.ll -side top -padx 5 -pady 5

	label .limits.lim.r.grippers.lbig -text "Big" -width 6 \
	    -background lightblue -relief groove -borderwidth 2
	pack .limits.lim.r.grippers.lbig -side top -padx 2 -pady 2

	frame .limits.lim.r.grippers.ebig -background blue
	pack .limits.lim.r.grippers.ebig -side top

	entry .limits.lim.r.grippers.ebig.min -width 5 \
	    -borderwidth 2 -textvariable bg_min
	pack .limits.lim.r.grippers.ebig.min -side left -padx 2 -pady 3
	
	label .limits.lim.r.grippers.ebig.l -text "-" \
	    -background blue -foreground yellow
	pack .limits.lim.r.grippers.ebig.l -side left -pady 2

	entry .limits.lim.r.grippers.ebig.max -width 5 \
	    -borderwidth 2 -textvariable bg_max
	pack .limits.lim.r.grippers.ebig.max -side left \
	    -padx 2 -pady 3

	label .limits.lim.r.grippers.lsmall -text "Small" -width 6 \
	    -background lightblue -relief groove -borderwidth 2
	pack .limits.lim.r.grippers.lsmall -side top -padx 2 -pady 2

	frame .limits.lim.r.grippers.esmall -background blue
	pack .limits.lim.r.grippers.esmall -side top

	entry .limits.lim.r.grippers.esmall.min -width 5 \
	    -borderwidth 2 -textvariable sg_min
	pack .limits.lim.r.grippers.esmall.min -side left \
	    -padx 2 -pady 2

	label .limits.lim.r.grippers.esmall.l -text "-" \
	    -background blue -foreground yellow 
	pack .limits.lim.r.grippers.esmall.l -side left -pady 2

	entry .limits.lim.r.grippers.esmall.max -width 5 \
	    -borderwidth 2 -textvariable sg_max
	pack .limits.lim.r.grippers.esmall.max -side right \
	    -padx 2 -pady 2

	button .limits.lim.r.apply -text " Apply " -background red2 \
	    -activebackground red -command SetLimits \
	    -borderwidth 3
	pack .limits.lim.r.apply -side bottom

	frame .limits.buttons -background blue \
	    -relief sunken -borderwidth 5
	pack  .limits.buttons -side bottom -pady 3

	button .limits.buttons.ask -borderwidth 5 \
	    -text " Ask server about limits " \
	    -background yellow2 -activebackground yellow \
	    -command AskServerAboutLimits
	pack .limits.buttons.ask -side left -padx 5 -pady 5

	button .limits.buttons.quit -text " Close " \
	    -borderwidth 5 -background green2 \
	    -activebackground green \
	    -command { destroy .limits }
	pack .limits.buttons.quit -side right -padx 5 -pady 5

    }

}



##################################################################
########      FUNCTION      ######################################
##################################################################

# this function sends command to grasp specified object

proc TakeObject { num } {

    SendCommand $::GRASP_OBJECT,$num
}


##################################################################
########      FUNCTION      ######################################
##################################################################

# this function sends command to release specified object

proc ReleaseObject { num } {

    SendCommand $::RELEASE_OBJECT,$num
}


##################################################################
########      FUNCTION      ######################################
##################################################################

# tis function allows autorepeating mode of arrow buttons

proc RepeatMove { num dummy } {

    global jointUpdate
    if { $jointUpdate == 0 } {
	set jointUpdate 1
	set scrlbar [ .main.tools.scrolls.s$num activate ]
	if { [ string equal $scrlbar "arrow1" ] } {
	    SendMoveCommand $num 1
	} 
	if { [ string equal $scrlbar "arrow2" ] } {
	    SendMoveCommand $num -1
	}
	update
	set jointUpdate 0
    }
}



##################################################################
########      FUNCTION      ######################################
##################################################################

# this function changes control panel for moving robot

proc ChangePanel { } {

    global panel 
    global controlmode
    global repeattime
    global qmin
    global qmax
    global q


    switch $panel {
	
	0 { destroy .main.tools.up
	    destroy .main.tools.down }
	1 { destroy .main.tools.scrolls }
	2 { destroy .main.tools.scales }
    }

    incr panel
    if { $controlmode != 1 && $panel == 2 } { incr panel } 
    set panel [ expr $panel % 3 ]
 
    
    switch  $panel {
	
	1 {
	    
	    frame .main.tools.scrolls -background blue
	    pack .main.tools.scrolls -side bottom
	    
	    for { set i 1 } { $i < 7 } { incr i } {   
		scrollbar .main.tools.scrolls.s$i  \
		    -background green2 -activebackground green \
		    -relief sunken -width 22 -troughcolor blue \
		    -elementborderwidth 3 -borderwidth 3 \
		    -command "RepeatMove $i" \
		    -repeatinterval $repeattime
		
		pack .main.tools.scrolls.s$i -side left \
		    -padx 5 -pady 4
	    }
	}
	    

	0 {
	    frame .main.tools.down -background blue
	    pack .main.tools.down -side bottom
	    frame .main.tools.up -background blue
	    pack .main.tools.up -side bottom -pady 2
	    
	    for { set i 1 } { $i < 7 } { incr i } {   
		button .main.tools.up.b$i -text "+" \
		    -background green2 -activebackground green \
		    -relief raised -width 2 \
		    -command "SendMoveCommand $i 1" 
		pack  .main.tools.up.b$i -side left
		button .main.tools.down.b$i -text "-" \
		    -background green2 -activebackground green \
		    -relief raised -width 2 \
		    -command "SendMoveCommand $i -1"
		pack  .main.tools.down.b$i -side left -pady 3
	    }

	}
    
	2 {
	    frame .main.tools.scales -background blue
	    pack .main.tools.scales -side bottom
	    
	    for { set i 0 } { $i < 6 } { incr i } {
		set x $i
		incr x
		scale .main.tools.scales.s$i \
		    -background green3 -activebackground green2 \
		    -orient vertical -resolution 1 -length 55 \
		    -from "$qmax($i)" -to "$qmin($i)" \
		    -sliderlength 15 -showvalue false \
		    -variable q($i) -troughcolor pink \
		    -command "UpdateJoint $x"
		pack  .main.tools.scales.s$i -side left \
		    -padx 8 -pady 3
	    }
  
	} 
    } 
}


##################################################################
########      FUNCTION      ######################################
##################################################################

# this function updates indicators of TK agent 
# (currently used only at the start of TK)

proc UpdateInfo { } {

    AskServerAboutJoints

}

##################################################################
########      FUNCTION      ######################################
##################################################################

# this function updates current values of joints/grippers 
# in the TK indicators

proc AskServerAboutJoints {} {

    global netsocket
    global connected
    global gripper
    global rpy
    global q

    . configure -cursor watch
    if { [winfo exists .gripper ] == 1 } { 
	.gripper configure -cursor watch 
    }
    update

    set command $::GET_INFO,$::JOINTS_INFO
    set len [ binary format "c" [ string length $command ] ]

    if { $connected == 0 } {
	SetConnection
	if { $connected == 1 } {
	    puts -nonewline $netsocket $len 
	    puts -nonewline $netsocket $command  
	    flush $netsocket
	    set num \x00
	    catch { set num [ read $netsocket 1 ] } dummyerror
	    binary scan $num "c" len
	    if { $len < 0 } { set len [ expr  $len + 256 ] }
	    if { $len != 0 } {
		set reply [ read $netsocket $len ]
		if { [ scan $reply\
		"%d %f %f %f %f %f %f %f %f %f %f %f %f %f" \
			   gripper j(0) j(1) j(2) j(3) j(4) j(5) \
			   j(6) \
			   rpy(0) rpy(1) rpy(2) \
			   rpy(3) rpy(4) rpy(5) ] == 14
		 } {
		    for { set i 3 } { $i < 6 } { incr i } { 
			set rpy($i) [ expr { 57.296 * $rpy($i) } ]
		    }
		    for { set i 0 } { $i < 6 } { incr i } { 
			set q($i) [ expr { 57.296 * $j($i) } ]
			set q($i) [ format "%3.2f" $q($i) ]
			set rpy($i) [ format "%4.1f" $rpy($i) ]
		    }

		    set q(6) $j(6)
		    CorrectGripper
		}
	    }
	    close  $netsocket
	}
	set connected 0 
    }
    . configure -cursor left_ptr
    if { [winfo exists .gripper ] == 1 } { 
	.gripper configure -cursor left_ptr 
    }
    update
}



##################################################################
########      FUNCTION      ######################################
##################################################################

# this function sends command to set new CPU timeout for server

proc SetCPU { dummy } {
    global cpu
    global cpuUpdate
    global scaleupdate
    if { $cpuUpdate == 0 } {
	set cpuUpdate 1
	after $scaleupdate \
	    { 
		set cputmp [ expr { $cpu * 1000 } ]
		SendCommand $::SET_CPU_IDLE,$cputmp
		set cpuUpdate 0 
	    }
    }
}

##################################################################
########      FUNCTION      ######################################
##################################################################

# this function sends command to server to set new value of
# network timeout

proc SetNetTimeout { dummy } {
    global net
    global scaleupdate
    global netUpdate
    if { $netUpdate == 0 } {
	set netUpdate 1
	after $scaleupdate \
	    { 
		set nettmp [ expr { $net * 1000 } ]
		SendCommand $::SET_NET_TIMEOUT,$nettmp
		set netUpdate 0 
	    }
    }
}


##################################################################
########      FUNCTION      ######################################
##################################################################

# this function sends command to update current value of
# specified joint

proc UpdateJoint { num dummy } {
    global q
    global scaleupdate
    global jointUpdate
    if { $jointUpdate == 0 } {
	set jointUpdate $num
	after $scaleupdate \
	    {   
		set com $::MOVE_JOINT_TO,$jointUpdate		
		for { set i 0 } { $i < 7 } { incr i } {
		    set p $q($i)
		    if { $i != 6 } {
			set p [ expr { 0.0174 * $p } ]
		    } 
		    set com $com,$p
		}
		SendCommand $com
		AskServerAboutJoints
		set jointUpdate 0  
	    }
    }
}




##################################################################
########      FUNCTION      ######################################
##################################################################

# this function sends movement command to robot server

proc SendMoveCommand { num sgn } {
  
    global controlmode
    global rpy_step
    global angle_step

    for { set i 1 } { $i < 9 } { incr i } {
	set p$i "0"  
    }
    
    if { $controlmode == 1 } { 
	
	set p$num [ expr { 0.0174 * $sgn * $angle_step } ]
	set prm $num,$p1,$p2,$p3,$p4,$p5,$p6,$p7,$p8
	SendCommand $::MOVE_JOINT_BY,$prm
	
    } else {
	
	if { $num < 4 } { 
	    set p$num [ expr { $sgn * $rpy_step } ] 
	} else {
	    set p$num [ expr { 0.0174 * $sgn * $angle_step } ]
	}
	set prm $num,$p1,$p2,$p3,$p4,$p5,$p6,$p7,$p8
	SendCommand $::MOVE_RPY_BY,$prm
    }

    AskServerAboutJoints

}


##################################################################
########      FUNCTION      ######################################
##################################################################

# this function changes indicator of connectivity 

proc RobotNotActive { } {
    .main.controlmenu.activity config -foreground yellow2 \
	-background red -text " ATTENTION : Robot is not active   " 
}


##################################################################
########      FUNCTION      ######################################
##################################################################

# this function changes indicator of connectivity 

proc RobotActive { } {
    .main.controlmenu.activity config -foreground black \
	-background green -text " ATTENTION : Robot is activated    "
}



##################################################################
########      FUNCTION      ######################################
##################################################################

# this is help function for sending commands to robot

proc SendCommand { command } {
    global netsocket
    global connected

    set len [ binary format "c" [ string length $command ] ]

    if { $connected == 0 } {
	SetConnection
	if { $connected == 1 } {
	    puts -nonewline $netsocket $len 
	    puts -nonewline $netsocket $command
	    close  $netsocket
	}
	set connected 0 
    }
}


##################################################################
########      FUNCTION      ######################################
##################################################################

# this function establishes connection between this client and server

proc SetConnection { } {
    global netsocket
    global servername
    global serverport
    global connected 

    if { [ catch { set netsocket \
		       [ socket $servername $serverport	] } ] } {
	RobotNotActive
	set connected 0 
    } else {
	RobotActive
	set connected 1
	fconfigure $netsocket -buffering line
    } 
}


##################################################################
########      FUNCTION      ######################################
##################################################################

# this function changes control mode of main control panel
# from RPY to Joints mode

proc ChangeModeToJoints { } {
    
    if { [ winfo exists .main.tools.rpylabels ] == 1 } {
	destroy .main.tools.rpylabels
    }

    if { [ winfo exists .main.tools.jointslabels ] != 1 } {
	
	frame .main.tools.jointslabels -background blue
	pack .main.tools.jointslabels -side top -padx 5 -pady 5

	for { set i 1 } { $i < 7 } { incr i } {
	    label .main.tools.jointslabels.l$i -background lightblue\
		-text $i -width 4 -relief groove
	    pack .main.tools.jointslabels.l$i -side left -padx 3
	}
    }
}



##################################################################
########      FUNCTION      ######################################
##################################################################

# this function changes control mode of main control panel
# from Joints to RPY mode

proc ChangeModeToRPY { } {

    global panel

    if { [ winfo exists .main.tools.jointslabels ] == 1 } {
	destroy .main.tools.jointslabels
    }

    if { [ winfo exists .main.tools.rpylabels] != 1 } {

	
	if { $panel == 2 } { ChangePanel }

	frame .main.tools.rpylabels -background blue
	pack .main.tools.rpylabels -side top -padx 5 -pady 5

	label .main.tools.rpylabels.l1 -background lightblue \
	    -text "X" -width 4 -relief groove 
	label .main.tools.rpylabels.l2 -background lightblue \
	    -text "Y" -width 4 -relief groove
	label .main.tools.rpylabels.l3 -background lightblue \
	    -text "Z" -width 4 -relief groove 
	label .main.tools.rpylabels.l4 -background lightblue \
	    -text "A" -width 4 -relief groove 
	label .main.tools.rpylabels.l5 -background lightblue \
	    -text "B" -width 4 -relief groove 
	label .main.tools.rpylabels.l6 -background lightblue \
	    -text "C" -width 4 -relief groove 

	for { set i  1 } { $i < 7 } { incr i } {
	    pack .main.tools.rpylabels.l$i -side left -padx 3
	}
    }
}


##################################################################
########      FUNCTION      ######################################
##################################################################

# this function sends command to change / put on specified gripper

proc ChangeGripper { x } {
    
    global gripper
    global qmin
    global qmax
    global bg_min
    global bg_max
    global sg_min
    global sg_max

    set gripper $x
    .main.controlmenu.state.grippers.b0 config -background red2 \
	-activebackground red
    .main.controlmenu.state.grippers.b1 config -background red2 \
	-activebackground red
    .main.controlmenu.state.grippers.b2 config -background red2 \
	-activebackground red
    .main.controlmenu.state.grippers.b3 config -background red2 \
	-activebackground red
    .main.controlmenu.state.grippers.b$x config -background green2 \
	-activebackground green

    switch $x {
	0 { SendCommand $::DETACH_BIG_GRIPPER 
	    SendCommand $::DETACH_SMALL_GRIPPER 
	    SendCommand $::DETACH_VACUUM_GRIPPER
	    set qmin(6) 0
	    set qmax(6) 0 }
	1 { SendCommand $::ATTACH_BIG_GRIPPER
	    set qmin(6) $bg_min
	    set qmax(6) $bg_max }
	2 { SendCommand $::ATTACH_SMALL_GRIPPER
	    set qmin(6) $sg_min
	    set qmax(6) $sg_max }
	3 { SendCommand $::ATTACH_VACUUM_GRIPPER
	    set qmin(6) 0
	    set qmax(6) 0 }
    }

    AskServerAboutJoints

}


##################################################################
########      FUNCTION      ######################################
##################################################################

# this function queries about current gripper on the robot 

proc CorrectGripper { } {
    
    global gripper
    global q
    global qmin
    global qmax
    global bg_min
    global bg_max
    global sg_min
    global sg_max

    .main.controlmenu.state.grippers.b0 config -background red2 \
	-activebackground red
    .main.controlmenu.state.grippers.b1 config -background red2 \
	-activebackground red
    .main.controlmenu.state.grippers.b2 config -background red2 \
	-activebackground red
    .main.controlmenu.state.grippers.b3 config -background red2 \
	-activebackground red
    .main.controlmenu.state.grippers.b$gripper config \
	-background green2 -activebackground green

    switch $gripper {
	0 { set qmin(6) 0
	    set qmax(6) 0 }
	1 { set qmin(6) $bg_min
	    set qmax(6) $bg_max }
	2 { set qmin(6) $sg_min
	    set qmax(6) $sg_max }
	3 { set qmin(6) 0
	    set qmax(6) 0 }
    }
    if { [ winfo exists .gripper.scale ] } {	
	.gripper.scale configure -from $qmin(6) -to $qmax(6)
	.gripper.scale set $q(6)
    }
}


##################################################################
########      FUNCTION      ######################################
##################################################################

# this function corrects scales of main control panel so
# that they reflect up to date joints values

proc CorrectScales { } {
    
    global panel
    global qmin
    global qmax
    global q
    
    if { $panel == 2 } {
	for { set i 0 } { $i < 6 } { incr i } {
	  .main.tools.scales.s$i configure \
	      -from "$qmax($i)" -to "$qmin($i)"
	  .main.tools.scales.s$i set $q($i)
	}
    }
}



##################################################################
########      FUNCTION      ######################################
##################################################################

# this function opens menu for control of some robot server parameters

proc ServerOptions { } {
    
    global cpu
    global net
    
    if { [ winfo exists .serveroptions ] != 1 } {
	
	toplevel .serveroptions -borderwidth 10 -background blue \
	    -relief raised
	wm title .serveroptions "Server Options"
	wm minsize .serveroptions 200 100
	
	label .serveroptions.l -background lightblue -relief groove \
	    -text " Configuration of robot server " \
	    -borderwidth 5
	pack  .serveroptions.l -side top -pady 5 -padx 5

	frame .serveroptions.scales -background lightblue \
	    -relief groove -borderwidth 2 
	pack .serveroptions.scales -side top -pady 5 -padx 5
	scale .serveroptions.scales.cpu -from 0 -to 100 \
	    -resolution 10 -background lightblue \
	    -orient horizontal -length 105 \
	    -label "CPU timeout ms" -showvalue true -variable cpu \
	    -command SetCPU
	pack  .serveroptions.scales.cpu -side left -padx 5 -pady 2
	
	scale .serveroptions.scales.net -from 0.1 -to 1000 \
	    -background lightblue \
	    -orient horizontal -length 105 \
	    -label "Net timeout ms" -showvalue true -variable net \
	    -command SetNetTimeout
	pack  .serveroptions.scales.net -side right -padx 5 -pady 2

	frame .serveroptions.buttons -background blue \
	    -relief sunken -borderwidth 5
	pack  .serveroptions.buttons -side bottom -pady 5

	button .serveroptions.buttons.close -text " Close " \
	    -relief raised -borderwidth 5 -background green2 \
	    -activebackground green \
	    -command { destroy .serveroptions }
	pack  .serveroptions.buttons.close -side top -pady 5 -padx 40


    }
}


##################################################################
########      FUNCTION      ######################################
##################################################################

# this function lets to choose "help" file

proc GetHelpFile { } {
    
    global helpfile
    set f [ tk_getOpenFile ]
    if [ file readable $f ] { set helpfile $f }
    
}


##################################################################
########      FUNCTION      ######################################
##################################################################

# this function opens menu to control various parameters

proc Settings { } {

    global servername
    global serverport
    global tmpname
    global tmpport
    global infoupdate
    global repeattime
    
    if { [ winfo exists .settings ] != 1 } {
	
	toplevel .settings -borderwidth 10 -background blue \
	    -relief raised 
	wm title .settings Configure
	wm minsize .settings 270 310
	
	label .settings.label -background lightblue -relief groove \
	    -text " Configuration for TCL/TK Client v1.0 " \
	    -borderwidth 5
	pack  .settings.label -side top -pady 5 -padx 2
	
	frame .settings.parameters -background blue
	pack  .settings.parameters -side top
	
	frame .settings.parameters.labels -borderwidth 2 \
	    -background lightblue -relief groove
	pack  .settings.parameters.labels -side left
	
	label .settings.parameters.labels.server \
	    -text "Server name" \
	    -background lightblue 
	pack  .settings.parameters.labels.server -side top
	
	label .settings.parameters.labels.port -text "Server port" \
	    -background lightblue 
	pack  .settings.parameters.labels.port -side top
	
	frame .settings.parameters.entries -borderwidth 5 \
	    -background blue
	pack  .settings.parameters.entries -side right
	
	entry .settings.parameters.entries.server -width 18 \
	    -relief sunken -textvariable name
	pack  .settings.parameters.entries.server -side top
	
	entry .settings.parameters.entries.port -width 18 \
	    -relief sunken -textvariable port
	pack  .settings.parameters.entries.port -side top

	button .settings.save -text " Activate settings " \
	    -background red2 -borderwidth 10 \
	    -activebackground red \
	    -command { set servername $name  
		set serverport $port 
		SetConnection 
		if { $connected == 1 } { close $netsocket }
		set connected 0
	    }
	pack .settings.save -side top -padx 2 -pady 5

	frame .settings.scales -background lightblue2 -relief \
	    groove -borderwidth 5 
	pack  .settings.scales -side top -pady 5

 	scale .settings.scales.update -from 200 -to 10000 \
	    -background lightblue2 -variable infoupdate \
	    -orient horizontal -length 200 -resolution 200 \
	    -label "Info update, ms" -showvalue true 
	pack  .settings.scales.update -side top -padx 5 -pady 2

	scale .settings.scales.repeat -from 10 -to 1000 \
	    -background lightblue2 -variable repeattime \
	    -orient horizontal -length 200 -resolution 10 \
	    -label "Autorepeat, ms" -showvalue true \
	    -command { ChangeAutorepeat }
	pack  .settings.scales.repeat -side top -padx 5 -pady 2

	frame .settings.lim_and_obj -background blue

	pack .settings.lim_and_obj -side top -padx 5 -pady 2

	menubutton .settings.lim_and_obj.lim -background yellow2 \
	    -activebackground yellow -borderwidth 3 \
	    -relief raised \
	    -text " Limits " -menu .settings.lim_and_obj.lim.m
	pack .settings.lim_and_obj.lim -side left -padx 10 -pady 2

	menu .settings.lim_and_obj.lim.m -background lightblue \
	    -activebackground yellow
	.settings.lim_and_obj.lim.m add command \
	    -label "Joints and grippers" -command Limits
	.settings.lim_and_obj.lim.m add command -label " Speeds " \
	    	-command Speeds

	button .settings.lim_and_obj.obj -background green2 \
	    -activebackground green -borderwidth 3 \
	    -text " Objects " -command Objects 
	pack .settings.lim_and_obj.obj -side left -padx 10 -pady 2

	frame .settings.buttons -background blue \
	    -relief sunken -borderwidth 5

	pack  .settings.buttons -side bottom -pady 5 -padx 3
	
	button .settings.buttons.serveropt -text "Server options" \
	    -command ServerOptions -borderwidth 5 -relief raised \
	    -background green2 -activebackground green 
	pack  .settings.buttons.serveropt -side left -padx 10 -pady 5
	
	button .settings.buttons.quit -text " Close " \
	    -borderwidth 5 -background green2 \
	    -activebackground green \
	    -command { destroy .settings }
	pack .settings.buttons.quit -side right -padx 10 -pady 5
	
    }
}


##################################################################
########      FUNCTION      ######################################
##################################################################

# this function opens menu to control gripping operations

proc GripperControl { } {
    
    global q
    global qmin
    global qmax
    if { [winfo exists .gripper ] != 1 } {
	toplevel .gripper -borderwidth 10 -background blue \
	    -relief raised
	wm title .gripper "Gripper Control"
	wm minsize .gripper 134 120
	
	label .gripper.label -text " Gripper Control " \
	    -background lightblue -relief groove  -borderwidth 5
	pack  .gripper.label -side top -pady 5

	scale .gripper.scale -from $qmin(6) -to $qmax(6) \
	    -background lightblue \
	    -orient horizontal -length 105 \
	    -label " Opening, mm" -showvalue true -variable q(6) \
	    -command { UpdateJoint 7 }
	pack  .gripper.scale -side top -padx 5 -pady 3

	label .gripper.objectslabel -text "  Object  " \
	    -background lightblue -relief groove \
	    -borderwidth 3
	pack .gripper.objectslabel -side top -padx 5 -pady 2

	frame .gripper.objects -background blue \
	    -borderwidth 2
	pack  .gripper.objects -side top -pady 0

	menubutton .gripper.objects.take -text " Take " \
	    -relief raised -background yellow2 \
	    -activebackground yellow -menu .gripper.objects.take.m
	pack .gripper.objects.take -side left -pady 2 -padx 3

	menu .gripper.objects.take.m -background lightblue \
	    -activebackground yellow
	
	menubutton .gripper.objects.put -text "  Put  " \
	    -relief raised -background yellow2 \
	    -activebackground yellow -menu .gripper.objects.put.m
	pack .gripper.objects.put -side left -pady 2 -padx 3

	menu .gripper.objects.put.m -background lightblue \
	    -activebackground yellow

	for { set i 0 } { $i < 10 } { incr i } {
	    .gripper.objects.put.m add command -label " N$i " \
		-command " ReleaseObject $i"
	    .gripper.objects.take.m add command -label " N$i " \
	    	-command " TakeObject $i"
	}


	frame .gripper.button -background blue \
	    -relief sunken -borderwidth 5
	pack  .gripper.button -side bottom -pady 2

	button .gripper.button.quit -text " Close " \
	    -borderwidth 5 -background green2 \
	    -activebackground green \
	    -command { destroy .gripper }
	pack .gripper.button.quit -side bottom -padx 10 -pady 2
	
    }
}


##################################################################
########      FUNCTION      ######################################
##################################################################

# this function opens "help" window

proc ShowHelp { } {
    
    global	helpfile
    set 	helpfd  0

    if { [winfo exists .help ] != 1 } {
    
	toplevel .help -borderwidth 10 -background blue \
	    -relief raised
	wm title .help Help
	wm minsize .help 80 20
	
	label .help.label -text " Help for TCL/TK Client v1.0 " \
	    -background lightblue -relief groove  -borderwidth 5
	pack  .help.label -side top -pady 10
	
	frame .help.buttons -borderwidth 5 -background blue \
	     -relief sunken 
	pack  .help.buttons -side bottom -padx 5 -pady 5
	
	button .help.buttons.quit -background lightblue2 \
	    -borderwidth 5 -command { destroy .help } -width 10 \
	    -activebackground lightblue -text Close
	pack   .help.buttons.quit -side right -padx 30 -pady 5
	
	button .help.buttons.file -text "Other File" \
	    -background lightblue2  -borderwidth 5  -width 10 \
	    -activebackground lightblue \
	    -command { GetHelpFile 
		.help.textframe.text delete 1.0 end
		set helpfd [ open $helpfile ]
		.help.textframe.text insert end [ read $helpfd ] 
		close $helpfd }
	pack .help.buttons.file -side left -padx 30 -pady 5
	
	frame .help.textframe 
	pack  .help.textframe -side top -padx 2 -fill both \
	    -expand true
	
	text  .help.textframe.text -width 75 -height 20 -font { fixed 14 } \
	    -relief sunken -background yellow -setgrid true \
	    -yscrollcommand { .help.textframe.scroll set } \
	    -borderwidth 10
	pack  .help.textframe.text -side left -fill both -expand true
	
	scrollbar .help.textframe.scroll  \
	    -command { .help.textframe.text yview }
	pack  .help.textframe.scroll -side right -fill y
	
	set helpfd [ open $helpfile ]
	.help.textframe.text insert end [ read $helpfd ] 
	close $helpfd
    }
}




##################################################################
##################################################################
############### global variables #################################
##################################################################
##################################################################



set helpfile    "../README.TK"

set gripper     0

# flag to update cpu usage
set cpuUpdate   0 
# flag to update net usage
set netUpdate   0 
# flag to change coordinates of the robot / number of joint
set jointUpdate 0 

# controlmode of 2 types : 1 = joints, 2 = rpy
set controlmode 1


set infoupdate  1000

# current joints values
set q(0)        0.0
set q(1)        0.0
set q(2)        0.0
set q(3)        0.0
set q(4)        0.0
set q(5)        0.0
set q(6)        0.0


# rpy coordinates of tool frame
set rpy(0)           0.0
set rpy(1)           0.0
set rpy(2)           0.0
set rpy(3)           0.0
set rpy(4)           0.0
set rpy(5)           0.0

# socket id
set netsocket   0

# connectivity flag
set connected   0

# activity flag
set scaleactive 0


# there are 3 types of panel : 0 = buttons, 1 = arrows, 2 = scales 
set panel       2

set repeattime  100


# object parameters

set F(0) 1
set F(1) 0
set F(2) 0
set F(3) 0

set F(4) 0
set F(5) 1
set F(6) 0
set F(7) 0

set F(8) 0
set F(9) 0
set F(10) 1
set F(11) 0

set F(12) 0
set F(13) 0
set F(14) 0
set F(15) 1

set redv 99
set bluev 99
set greenv 99

set sizex 0
set sizey 0
set sizez 0

set centerx 0
set centery 0
set centerz 0


source "settings.tcl"

set tmpname     $servername
set tmpport     $serverport


##################################################################
##################################################################
##################################################################
########              MAIN    FUNCTION               #############
##################################################################
##################################################################
##################################################################


wm minsize . 284 315
wm geometry . 284x490+0-0
wm title . TK_Client

frame	.main -borderwidth 10 -background blue -relief raised
pack  	.main -side top -expand true -fill both

label   .main.label -text " TCL/TK Client for Robot Simulator v1.0 "\
	-background lightblue -relief groove  -borderwidth 5
pack  	.main.label -side top -pady 5

frame 	.main.buttons -borderwidth 5 -background blue -relief sunken
pack  	.main.buttons -side bottom -padx 3 -pady 3

button 	.main.buttons.quit -text Quit -command exit \
    -background red2 \
    -borderwidth 5 -activebackground red
pack	.main.buttons.quit -side right -padx 7 -pady 5 

button 	.main.buttons.help -text Help -command { ShowHelp } \
	-background yellow2 -borderwidth 5 -activebackground yellow
pack	.main.buttons.help -side left -padx 6 -pady 5

button  .main.buttons.settings -text Configure \
    -background green2  -borderwidth 5 -activebackground green \
    -command { set name $servername
	set port $serverport 
	Settings } 

pack	.main.buttons.settings -side left -padx 7 -pady 5

############# control menu #########################################

frame .main.controlmenu -background blue -relief groove \
      -borderwidth 5 
pack  .main.controlmenu -side top -padx 5 

label .main.controlmenu.activity -borderwidth 3 \
    -foreground yellow2 -background red -relief raised \
    -text " ATTENTION : Robot is not active   " 
pack  .main.controlmenu.activity -side top -padx 10 -pady 3


########## state of robot ##########################################

frame .main.controlmenu.state -background blue 
pack  .main.controlmenu.state -side top -padx 5 -pady 3

########## R P Y  values ########################################### 

frame .main.controlmenu.state.rpy -background blue  
pack  .main.controlmenu.state.rpy -side left 

label .main.controlmenu.state.rpy.l -text  "  R P Y  " \
    -relief groove -background lightblue
pack  .main.controlmenu.state.rpy.l -side top -padx 5 -pady 5

frame .main.controlmenu.state.rpy.p -background blue  
pack  .main.controlmenu.state.rpy.p -side bottom 

frame .main.controlmenu.state.rpy.p.l -relief groove \
    -borderwidth 2 -background lightblue  
pack  .main.controlmenu.state.rpy.p.l -side left

label .main.controlmenu.state.rpy.p.l.l0 -text  "X " \
    -background lightblue
label .main.controlmenu.state.rpy.p.l.l1 -text  "Y " \
    -background lightblue
label .main.controlmenu.state.rpy.p.l.l2 -text  "Z " \
    -background lightblue
label .main.controlmenu.state.rpy.p.l.l3 -text  "A " \
    -background lightblue
label .main.controlmenu.state.rpy.p.l.l4 -text  "B " \
    -background lightblue
label .main.controlmenu.state.rpy.p.l.l5 -text  "C " \
    -background lightblue

frame .main.controlmenu.state.rpy.p.v -background blue \
    -relief groove -borderwidth 2  
pack  .main.controlmenu.state.rpy.p.v -side left -padx 2

for { set i 0 } { $i < 6 } { incr i } {
    pack  .main.controlmenu.state.rpy.p.l.l$i -side top -padx 2
    label .main.controlmenu.state.rpy.p.v.v$i \
	-relief sunken -width 6 -textvariable rpy($i)
    pack  .main.controlmenu.state.rpy.p.v.v$i -side top
}

###### joint values ################################################

frame .main.controlmenu.state.joints -background blue
pack  .main.controlmenu.state.joints -side left -padx 3

label .main.controlmenu.state.joints.l -text " Joints  " \
    -relief groove -background lightblue
pack  .main.controlmenu.state.joints.l -side top -padx 5 -pady 5

frame .main.controlmenu.state.joints.p -background blue  
pack  .main.controlmenu.state.joints.p -side bottom 

frame .main.controlmenu.state.joints.p.l -background lightblue \
    -relief groove -borderwidth 2
pack  .main.controlmenu.state.joints.p.l -side left

frame .main.controlmenu.state.joints.p.v -background blue \
    -relief groove -borderwidth 2
pack  .main.controlmenu.state.joints.p.v -side left -padx 2

for { set i 1 } { $i < 7 } { incr i } {
    label .main.controlmenu.state.joints.p.l.l$i -text  " $i " \
	-background lightblue
    pack  .main.controlmenu.state.joints.p.l.l$i -side top
    
    set x $i
    incr x -1

    label .main.controlmenu.state.joints.p.v.v$i -textvariable q($x)\
	-relief sunken -width 6
    pack  .main.controlmenu.state.joints.p.v.v$i -side top
}

###### grippers ###################################################

frame .main.controlmenu.state.grippers -background blue
pack  .main.controlmenu.state.grippers -side right 

label .main.controlmenu.state.grippers.l -text " Grippers" \
    -relief groove -background lightblue
pack  .main.controlmenu.state.grippers.l -side top -padx 5 -pady 4

button .main.controlmenu.state.grippers.b1 -text "Big" \
    -relief raised -background red2 -width 5 \
    -activebackground red  -command { ChangeGripper 1 }
pack  .main.controlmenu.state.grippers.b1 -side top -padx 5 -pady 1

button .main.controlmenu.state.grippers.b2 -text "Small" \
    -relief raised -background red2 -width 5 \
    -activebackground red  -command { ChangeGripper 2 }
pack  .main.controlmenu.state.grippers.b2 -side top -padx 5 -pady 1

button .main.controlmenu.state.grippers.b3 -text "Vacuum" \
    -activebackground red  -command { ChangeGripper 3 } \
    -relief raised -background red2 -width 5 
pack  .main.controlmenu.state.grippers.b3 -side top -padx 5 -pady 1

button .main.controlmenu.state.grippers.b0 -text "Off" \
    -relief raised -background green2 -width 5 \
    -activebackground green  -command { ChangeGripper 0 }
pack  .main.controlmenu.state.grippers.b0 -side top -padx 5 -pady 1

frame .main.middle -background blue
pack  .main.middle -side top

radiobutton .main.middle.b1 -text "RPY mode" -value 2 \
    -relief raised -background lightblue2 -variable controlmode \
    -activebackground lightblue -command { ChangeModeToRPY }
pack  .main.middle.b1 -side left -pady 5 -padx 3

radiobutton .main.middle.b2 -text "Joint mode" -value 1 \
    -relief raised -background lightblue2 -variable controlmode \
    -activebackground lightblue -command { ChangeModeToJoints }
pack  .main.middle.b2 -side left -pady 5 -padx 3

frame .main.tools -background blue -relief groove \
    -borderwidth 4
pack .main.tools -side top

frame .main.stepcontrol -background blue
pack  .main.stepcontrol -side bottom

button .main.stepcontrol.grippercontrol -text " Gripping " \
    -relief raised -background green2 \
    -activebackground green -command { GripperControl }
pack  .main.stepcontrol.grippercontrol -side right -pady 5 -padx 3

button .main.stepcontrol.controlmode -text "Change panel" \
    -relief raised -background green2 \
    -activebackground green -command { ChangePanel }
pack  .main.stepcontrol.controlmode -side right -pady 5 -padx 3

menubutton .main.stepcontrol.step -text " Step " \
    -relief raised -background lightblue2 \
    -activebackground lightblue -menu .main.stepcontrol.step.menu
pack  .main.stepcontrol.step -side left -pady 5 -padx 3

menu .main.stepcontrol.step.menu -background yellow \
    -activebackground lightblue
.main.stepcontrol.step.menu add cascade -label linear \
    -menu .main.stepcontrol.step.menu.linear
.main.stepcontrol.step.menu add separator
.main.stepcontrol.step.menu add cascade -label rotation \
    -menu .main.stepcontrol.step.menu.rotation

menu .main.stepcontrol.step.menu.linear -background yellow \
    -activebackground lightblue

.main.stepcontrol.step.menu.linear add radio -label "1 mm" \
    -variable rpy_step -value 1
.main.stepcontrol.step.menu.linear add radio -label "2 mm" \
    -variable rpy_step -value 2
.main.stepcontrol.step.menu.linear add radio -label "5 mm" \
    -variable rpy_step -value 5
.main.stepcontrol.step.menu.linear add radio -label "10 mm" \
    -variable rpy_step -value 10
.main.stepcontrol.step.menu.linear add radio -label "20 mm" \
    -variable rpy_step -value 20
.main.stepcontrol.step.menu.linear add radio -label "50 mm" \
    -variable rpy_step -value 50
.main.stepcontrol.step.menu.linear add radio -label "100 mm" \
    -variable rpy_step -value 100
.main.stepcontrol.step.menu.linear add radio -label "200 mm" \
    -variable rpy_step -value 200


menu .main.stepcontrol.step.menu.rotation -background yellow \
    -activebackground lightblue

.main.stepcontrol.step.menu.rotation add radio -label "1 deg" \
    -variable angle_step -value 1
.main.stepcontrol.step.menu.rotation add radio -label "2 deg" \
    -variable angle_step -value 2
.main.stepcontrol.step.menu.rotation add radio -label "5 deg" \
    -variable angle_step -value 5
.main.stepcontrol.step.menu.rotation add radio -label "10 deg" \
    -variable angle_step -value 10
.main.stepcontrol.step.menu.rotation add radio -label "15 deg" \
    -variable angle_step -value 15


################# starting some routines ############################

ChangeModeToJoints
ChangePanel

SetConnection
set connected 0

catch { close $netsocket } 

source "command_type.tcl"

. configure -cursor watch

after 3000 UpdateInfo

