#source random.tcl

#--------------------------#
#  Initialize variables    #
#--------------------------#
set veh_name ""
#set vehx 100  Dave Removed these lines as the vehicle would reset position on redraw
#set vehy 100
set vframe .ventana.editor.veh_frame
set check_var 1
set vehicle_timer 0
set veh_loop_id 0

proc get_window_path_name {name} {

   global veh_name
   set veh_name $name
}


proc set_vehicle_pos { x y } {
   global vehx 
   global vehy

   set vehx $x
   set vehy $y
}



proc display_vehicle_or_not { flag } {

	global vehx
	global vehy
	global veh
	global veh_name
	global vframe
	global check_var
	
	#----------------
	# Stop Polling
	#----------------
	
	
	if {$vehx == "" } { set vehx 0 }
	if {$vehy == "" } { set vehy 0 }

	set vframe .ventana.editor.veh_frame

	
        if {$flag == 1} {
		if [winfo exists $vframe] { destroy $vframe }
		set check_var 1
		frame $vframe -background black -width 200 -height 360
		place $vframe -x $vehx -y $vehy
		
		
		# Draw the special "togl" canvas here
		set veh $vframe.vehicle$veh_name
		togl $veh -width 200 -height 200 -rgba true -depth true -double true  \
			-privatecmap false
		place $veh -x 0 -y 0

		
  		display_vehicle
    		display_veh_info

	} else {
		set check_var 0
 		destroy $vframe
    	}
    	
}




#-------------------------------------------------------------------
# This procedure display the vehicle and its cameras
#
# The vehicle can be manipulated using commands :
#    lateral_veh ---> left / right
#    vertical_veh ---> up / down
#    pitch_veh ---> tilting up / down
#    roll_veh ---> roll left / right
#    camh1_veh ---> rotate camera 1 horizontally
#		( '+' ClockWise and  '-' CounterClockWise )
#    camv1_veh ---> rotate camera 1 vertically
#		( '+' up and '-' down )
#    camh2_veh ---> rotate camera 2 horizontally
#		( '+' ClockWise and  '-' CounterClockWise )
#    camv2_veh ---> rotate camera 2 vertically
#		( '+' up and '-' down )
#----------------------------------------------------------------


proc display_vehicle { } {
   #-----------------
   # Declare variables
   #-----------------
   global veh_name
   global vframe
   global check_var  vehicle_timer
   global pitch roll cam1_pan cam1_tilt cam2_pan cam2_tilt cam1_shoulder rpm
   global cam1_pan_angle cam1_tilt_angle cam1_shoulder_angle
   global lat_thrust vert_thrust hor_thrust
	

   #------------------
   # Setting variables
   #------------------
   set veh $vframe.vehicle$veh_name
   if ![winfo exists $veh] {return}

   if {$check_var == 0} {
   	return
   }

   #--------------
   # update the timer
   #-------------
   incr vehicle_timer
   if { $vehicle_timer >= 200 } {
   	redisplay_vehicle
   	return
   }


   #--------------------------------
   # try to manipulate the graphic
   #--------------------------------

   #puts "\[commands\] : lateral_veh  camh1_veh camh2_veh  "
   #puts "             camv1_veh camv2_veh pitch_veh roll_veh vertical_veh"
   #puts "usage   : \$veh \[commands\] \[some_number\]"
   #puts "example : \$veh lateral_veh -30 "

   set str [split [get_vehicle_act] |]
   set sit [split [get_sit_camera_angle] |]
   set sony_angle [split [get_sony_camera_angle] |]
   set sony_display [split [get_sony_camera_display] |]

   #puts "str : $str"
   #puts "sit : $sit"


   set pitch [get_pitch]
   set roll [get_roll]

   #---------------------#
   # main camera (sony)  #
   #---------------------#

   # The Pan and Tilt Feedbacks are centered around 180 degrees so must subtract pi

   set cam1_pan [lindex $sony_display 0]
   set cam1_tilt [lindex $sony_display 1]
   set cam1_shoulder [lindex $sony_display 2] 	

   # The camera angle to be displayed in box

   set cam1_pan_angle [lindex $sony_display 0]
   set cam1_tilt_angle [lindex $sony_display 1]
   set cam1_shoulder_angle [lindex $sony_display 2]


   #---------------------#
   # SIT camera          #
   #---------------------#

   set cam2_pan [lindex $sit 0]
   set cam2_tilt [lindex $sit 1]	

   #----------------------#
   # RPM                  #
   #----------------------#
   set temp_rpm [get_rpm_speed]
   set rpm [expr $temp_rpm *.017]

   #--------------------#
   # Thruster command 	#
   #--------------------#

   set stbd_thrust [lindex $str 0]
   set port_thrust [lindex $str 1]
   set vert_thrust [lindex $str 2]
   set lat_thrust [lindex $str 3]


   #----------------------------#
   # Now, animate the vehicle	#
   #----------------------------#

   $veh pitch_veh $pitch
   $veh roll_veh $roll
   $veh camv1_veh $cam1_pan
   $veh camh1_veh $cam1_tilt
   $veh camv2_veh $cam2_pan
   $veh camh2_veh $cam2_tilt
   $veh lateral_veh $lat_thrust
   $veh vertical_veh $vert_thrust
   $veh port_veh $port_thrust
   $veh stbd_veh $stbd_thrust

   #puts "vehicle"

   global veh_loop_id
   set veh_loop_id [after 200 [list display_vehicle]]

}

proc display_veh_info { } {
	global vframe
	global pitch roll cam1_pan_angle cam1_tilt_angle cam1_shoulder_angle cam2_pan cam2_tilt rpm
	global lat_thrust vert_thrust hor_thrust
	
 	# Pitch
	label $vframe.pitch_label -text "Pitch" -fg white -bg black -font {Halvetica 10}
	place $vframe.pitch_label -x 0 -y 200
	entry $vframe.pitch -textvariable pitch -bg black -state disabled \
		-fg white -relief flat -font {Halvetica 10} -width 7
	place $vframe.pitch -x 40 -y 200 -height 15
	
	# Roll
	label $vframe.roll_label -text "Roll" -fg white -bg black -font {Halvetica 10}
	place $vframe.roll_label -x 100 -y 200
	entry $vframe.roll -textvariable roll -bg black -state disabled \
		-fg white -relief flat -font {Halvetica 10} -width 7
	place $vframe.roll -x 140 -y 200 -height 15
	
	# Camera 1
	label $vframe.cam1_label -text "Camera 1" -fg yellow -bg black -font {Halvetica 10}
	place $vframe.cam1_label -x 0 -y 220
	# Camera 1 Pan
	label $vframe.cam1_pan_label -text "Pan" -fg white -bg black -font {Halvetica 10}
	place $vframe.cam1_pan_label -x 0 -y 240
	entry $vframe.cam1_pan -textvariable cam1_pan_angle -bg black -state disabled \
		-fg white -relief flat -font {Halvetica 10} -width 7
	place $vframe.cam1_pan -x 40 -y 240 -height 15
	# Camera 1 Tilt
	label $vframe.cam1_tilt_label -text "Tilt" -fg white -bg black -font {Halvetica 10}
	place $vframe.cam1_tilt_label -x 100 -y 240
	entry $vframe.cam1_tilt  -textvariable cam1_tilt_angle -bg black -state disabled \
		-fg white -relief flat -font {Halvetica 10} -width 7
	place $vframe.cam1_tilt  -x 140 -y 240 -height 15
	# Camera 1 Shoulder
	label $vframe.cam1_shoulder_label -text "Shoulder" -fg white -bg black -font {Halvetica 10}
	place $vframe.cam1_shoulder_label -x 0 -y 260
	entry $vframe.cam1_shoulder  -textvariable cam1_shoulder_angle -bg black -state disabled \
		-fg white -relief flat -font {Halvetica 10} -width 7
	place $vframe.cam1_shoulder  -x 70 -y 260 -height 15
	
	
	# Camera 2
	label $vframe.cam2_label -text "Camera 2" -fg yellow -bg black -font {Halvetica 10}
	place $vframe.cam2_label -x 0 -y 280
	# Camera 2 Pan
	label $vframe.cam2_pan_label -text "Pan" -fg white -bg black -font {Halvetica 10}
	place $vframe.cam2_pan_label -x 0 -y 300
	entry $vframe.cam2_pan -textvariable cam2_pan -bg black -state disabled \
		-fg white -relief flat -font {Halvetica 10} -width 7
	place $vframe.cam2_pan -x 40 -y 300 -height 15
	# Camera 2 Tilt
	label $vframe.cam2_tilt_label -text "Tilt" -fg white -bg black -font {Halvetica 10}
	place $vframe.cam2_tilt_label -x 100 -y 300
	entry $vframe.cam2_tilt  -textvariable cam2_tilt -bg black -state disabled \
		-fg white -relief flat -font {Halvetica 10} -width 7
	place $vframe.cam2_tilt  -x 140 -y 300 -height 15
	
	# RPM
	label $vframe.rpm_label -text "RPM" -fg white -bg black -font {Halvetica 10}
	place $vframe.rpm_label -x 0 -y 340
	entry $vframe.rpm -textvariable rpm -bg black -state disabled \
		-fg white -relief flat -font {Halvetica 10} -width 7
	place $vframe.rpm -x 40 -y 340 -height 15
	            	
}


proc redisplay_vehicle { } {
             global check_var
             global vehicle_timer
             global vframe

	if {$check_var == 1} {
		set vehicle_timer 0
		destroy $vframe
 		display_vehicle_or_not  1
 		return
   	}
}