#!/bin/sh
# @(#) $Header: /home/abrown/public_html/first/first2002chs/software2002/simulator/RCS/menace.tcl,v 1.16 2006/12/14 21:19:03 abrown Exp $
# \
exec wish "$0" -geometry "425x560" "$@"

# -----------------------------------------------------------------
# menace.tcl is a TCL program designed to exercise the CHS bsx
# code.  It is in no way essential to the pb2c translator.
# Rather it is an example of how to use the result of the
# translation.
#
# Copyright (C) 2002  Allen Brown
# 
# This program is free software; you can redistribute it and/or modify
# it under the terms of the GNU General Public License version 2 as
# published by the Free Software Foundation.
# 
# This program is distributed in the hope that it will be useful,
# but WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
# GNU General Public License for more details.
# 
# You should have received a copy of the GNU General Public License
# along with this program; if not, write to the
#   Free Software Foundation, Inc.
#   59 Temple Place, Suite 330
#   Boston, MA  02111-1307  USA
# 
# To contact the author of this software:
#   Allen Brown
#   PO Box J
#   Corvallis, OR
# 
#   http://brown.armoredpenguin.com/~abrown/contact.html
# -----------------------------------------------------------------

# running
  set state 0; # 0=stop, 1=run
  set clock 1

# initial values
  set errors 0
  set cycle 0
  set joyX 127
  set joyY 128
  set tractL 128
  set tractR 128

# locations of graphics
  set posCanvasX 10
  set posCanvasY 0
  set sizCanvasX 256
  set sizCanvasY 256
  set posJoysX 20
  set posJoysY 270
  set posButtonsX 20
  set posButtonsY 485
  set padX 7
  set padY 3

# ---- Function to update the graphics ------------
proc updatePos {} \
{
  global cycle logfileID pipeID
  global joyItem joyX joyY pot1 pot2
  global tractLItem tractRItem tractL tractR
  global state
  global clock
  global oi_swA oi_swB sw1 sw2 sw3 sw4 sw5 pb1 pb2 pb3 pb4 p2_sw_trig
  global rc_swA rc_sw1 rc_sw2 rc_sw6 rc_sw7 rc_sw8 rc_sw9
  global ballDumperL ballDumperR dumperStateL dumperStateR
  global ballGatherF ballGatherR
  global goalGrabberUM goalGrabberUA goalGrabberLM goalGrabberLA goalState
  if { $cycle < 1 }\
  {
    set rc_sw1 0
    set rc_sw2 0
    set rc_sw6 0
    set rc_sw7 0
    set rc_sw8 0
    set rc_sw9 0
    set rc_swB 0
    set goalState 0
    set dumperStateL 0.0
    set dumperStateR 0.0
  }
  set rc_swA [expr $rc_sw1 + $rc_sw2 + $rc_sw6 + $rc_sw7 + $rc_sw8]
  set rc_swB [expr $rc_sw9]
  set oi_swA [expr $sw1 + $sw2 + $sw3 + $pb1 + $pb2 + $pb3 + $pb4]
  set oi_swB [expr $p2_sw_trig]
  puts $pipeID "$joyX $joyY $pot1 $oi_swA $oi_swB $rc_swA $rc_swB"
  puts "tcl: $joyX $joyY $pot1 $oi_swA $oi_swB $rc_swA $rc_swB"
  flush $pipeID
  set token "unmatched"
  while { $token != "pbserout:" }\
  {
    gets $pipeID pipeout
    puts $logfileID $pipeout
    set linearray [split $pipeout]
    set token [lindex $linearray 0]
    if { $token == "output:" }\
    { # This is a disgusting kludge.
      set outputword [lindex $linearray 1]
      set outbit8 [expr !!($outputword & 1)]
      set outbit9 [expr !!($outputword & 2)]
      set outbit10 [expr !!($outputword & 4)]
      set outbit11 [expr !!($outputword & 8)]
      set outbit12 [expr !!($outputword & 16)]
      set outbit13 [expr !!($outputword & 32)]
      set outbit14 [expr !!($outputword & 64)]
      set outbit15 [expr !!($outputword & 128)]
    }
  }
  puts "$pipeout"
  regexp {fld3=([0-9]*) *fld4=([0-9]*) *fld5=([0-9]*) *fld6=([0-9]*) *fld7=([0-9]*) *fld8=([0-9]*) *fld9=([0-9]*) *fld10=([0-9]*)} \
	  $pipeout token pwm1 relayA pwm2 dummy pwm3 pwm4 pwm5 pwm6
  TractorMotors $pwm2 $pwm1 $joyX $joyY $joyItem $tractLItem $tractRItem
  GoalGrab $relayA $goalGrabberUM $goalGrabberUA $goalGrabberLM $goalGrabberLA
  BallGather $pwm4 $pwm3 $ballGatherF $ballGatherR
  BallDumper $dumperStateL $dumperStateR $ballDumperL $ballDumperR
  set dumperStateL [expr $dumperStateL - (127-$pwm5)/120.0]
  set dumperStateR [expr $dumperStateR + (127-$pwm6)/120.0]
  puts "X=$joyX Y=$joyY L=$tractL R=$tractR"
  puts $logfileID "$joyX $joyY $tractL $tractR"
  flush $logfileID
  LED   ./red1Button $outbit9  "#ff0000" "#600000"
  LED ./green1Button $outbit8  "#00ff00" "#006000"
  LED   ./red2Button $outbit12 "#ff0000" "#600000"
  LED ./green2Button $outbit13 "#00ff00" "#006000"
  if {$state} then \
  { # Run
    after [expr 1000 / $clock] updatePos
    ./runButton configure -state disabled
    ./stopButton configure -state normal
  } else { # Stop
    ./runButton configure -state normal
    ./stopButton configure -state disabled
  }
  set cycle [expr $cycle + 1]
}; # updatePos

# ---- Events ------------
proc runButtonEvent {} \
{
  global cycle logfileID pipeID
  global joyItem joyX joyY pot1 pot2
  global tractLItem tractRItem tractL tractR
  global state
  global clock
  global oi_swA oi_swB sw1 sw2 sw3 sw4 sw5 pb1 pb2 pb3 pb4 p2_sw_trig
  global rc_swA rc_sw1 rc_sw2 rc_sw6 rc_sw7 rc_sw8 rc_sw9
  global ballDumperL ballDumperR dumperStateL dumperStateR
  global ballGatherF ballGatherR
  global goalGrabberUM goalGrabberUA goalGrabberLM goalGrabberLA goalState
  set state 1
  updatePos
}; # runButtonEvent

# -------------------------
proc stopButtonEvent {} \
{
  global state
  set state 0
}; # stopButtonEvent

# -------------------------
proc stepButtonEvent {} \
{
  global cycle logfileID pipeID
  global joyItem joyX joyY pot1 pot2
  global tractLItem tractRItem tractL tractR
  global state
  global clock
  global oi_swA oi_swB sw1 sw2 sw3 sw4 sw5 pb1 pb2 pb3 pb4 p2_sw_trig
  global rc_swA rc_sw1 rc_sw2 rc_sw6 rc_sw7 rc_sw8 rc_sw9
  global ballDumperL ballDumperR dumperStateL dumperStateR
  global ballGatherF ballGatherR
  global goalGrabberUM goalGrabberUA goalGrabberLM goalGrabberLA goalState
  set state 0
  updatePos
}; # stepButtonEvent

# -------------------------
proc exitButtonEvent {} \
{
  global logfileID
  close $logfileID
  destroy .
}; # exitButtonEvent

# -------------------------
proc LED { {LEDname} {value} {oncolor} {offcolor} } \
{
  if {$value == 0} then \
  {
    $LEDname configure -background $offcolor -activebackground $offcolor
  } else {
    $LEDname configure -background $oncolor -activebackground $oncolor
  }
}; # LED

# -------------------------
proc TractorMotors { {leftMotor} {rightMotor} {xJoy} {yJoy} \
	{joyItem} {tractLItem} {tractRItem} } \
{
  set tractL [expr (256-$leftMotor)]
  set tractR [expr (256-$rightMotor)]
  ./tractorView insert $joyItem 2 "$xJoy [expr 256-$yJoy]"
  ./tractorView dchars $joyItem 4
  ./tractorView insert $tractLItem 2 "63 $tractL"
  ./tractorView dchars $tractLItem 4
  ./tractorView insert $tractRItem 2 "192 $tractR"
  ./tractorView dchars $tractRItem 4
}; # TractorMotors

# -------------------------
proc GoalGrab { {relayA} {goalGrabberUM} {goalGrabberUA} \
	{goalGrabberLM} {goalGrabberLA} } \
{
  global cycle rc_sw1 rc_sw2 goalState

  if { [expr !!($relayA & 1)] }\
  {
    set goalState [expr $goalState + 1]
  }
  if { [expr !!($relayA & 2)] }\
  {
    set goalState [expr $goalState - 1]
  }
   set goalMove [expr $goalState / 10]
  ./dumperView insert $goalGrabberUM 2 "111 [expr 180 + $goalMove]"
  ./dumperView dchars $goalGrabberUM 4 6
  ./dumperView insert $goalGrabberUA 0 "111 [expr 180 + $goalMove]
					 90 [expr 160 + $goalMove]"
  ./dumperView dchars $goalGrabberUA 4 8
  ./dumperView insert $goalGrabberLM 2 "111 [expr 180 - $goalMove]"
  ./dumperView dchars $goalGrabberLM 4 6
  ./dumperView insert $goalGrabberLA 0 "111 [expr 180 - $goalMove]
					 90 [expr 200 - $goalMove]"
  ./dumperView dchars $goalGrabberLA 4 8

  if { $goalState > 120 } then \
  {
    set rc_sw1 1
  } else {
    set rc_sw1 0
  }
  if { $goalState < 3 } then \
  {
    set rc_sw2 2
  } else {
    set rc_sw2 0
  }
}; # GoalGrab

# -------------------------
proc BallGather { {fMotor} {rMotor} {ballGatherF} {ballGatherR} } \
{
  ./dumperView insert $ballGatherF 2 "[expr 06 + ($fMotor-127) / 1.6] 255"
  ./dumperView dchars $ballGatherF 4 6
  ./dumperView insert $ballGatherR 2 "[expr 56 + ($rMotor-127) / 1.6] 255"
  ./dumperView dchars $ballGatherR 4 6
}; # BallGather

# -------------------------
proc BallDumper { {leftState} {rightState} {ballDumperL} {ballDumperR} } \
{
  global cycle rc_sw6 rc_sw9 rc_sw7 rc_sw8

  ./dumperView insert $ballDumperL 2 "11 [expr 115 - $leftState]"
  ./dumperView dchars $ballDumperL 4 6
  ./dumperView insert $ballDumperR 2 "31 [expr 115 - $rightState]"
  ./dumperView dchars $ballDumperR 4 6

  # Limit switches
  if { $rightState > 100 } then \
  {
    set rc_sw6 32
  } else {
    set rc_sw6 0
  }
  if { $leftState > 102 } then \
  {
    set rc_sw9 1
  } else {
    set rc_sw9 0
  }
  if { $leftState < 3 } then \
  {
    set rc_sw7 64
  } else {
    set rc_sw7 0
  }
  if { $leftState < [expr $rightState + $rc_sw8/16] } then \
  {
    set rc_sw8 128
  } else {
    set rc_sw8 0
  }
}; # BallDumper

#================= Main ================================
set executable "./vars"
while { "" != $argv }\
{
  #puts "argv='$argc'"
  if [regexp {^-e ([-_\.\\/,a-zA-Z0-9]+)(.*)} $argv dummy executable argv] \
  {
  } else {
    regexp {^([^ ]+) *} $argv dummy token
    regexp {^[^ ]+ *(.*)} $argv {} argv
    puts "Unrecognized option: '$token'."
    incr errors
  }
}; # Parse the command line

# ---- Playing Field ------------
canvas ./tractorView -borderwidth 3 -background {#7f8f7f} \
  -height $sizCanvasY -width $sizCanvasX
place ./tractorView -x [expr $posCanvasX + 0] -y [expr $posCanvasY + 0]
set joyItem [./tractorView create line 127 127 $joyX $joyY \
  -fill "#000000" -width 4 -arrow last]
set tractLItem [./tractorView create line 64 127 63 $tractL \
  -fill "#ff0000" -width 4 -arrow last]
set tractRItem [./tractorView create line 191 127 192 $tractR \
  -fill "#ff0000" -width 4 -arrow last]
set posCanvasX [expr $posCanvasX + 270]

# ---- Ball Stuff ------------
canvas ./dumperView -borderwidth 3 -background {#7f8f7f} \
  -height $sizCanvasY -width 130
place ./dumperView -x [expr $posCanvasX + 0] -y [expr $posCanvasY + 0]
set posCanvasX [expr $posCanvasX + 270]
set ballDumperL [./dumperView create line 10 120 11 121 \
	-fill "#000000" -width 4]
set ballDumperR [./dumperView create line 30 120 31 121 \
	-fill "#000000" -width 4]
set goalGrabberUM [./dumperView create line 40 180 111 181 \
	-fill "#000000" -width 4]
set goalGrabberUA [./dumperView create line 111 181 90 160 \
	-fill "#000000" -width 4]
set goalGrabberLM [./dumperView create line 40 180 111 181 \
	-fill "#000000" -width 4]
set goalGrabberLA [./dumperView create line 111 181 90 200 \
	-fill "#000000" -width 4]
set ballGatherF [./dumperView create line 5 240 81 255 \
	-fill "#000000" -width 4 -arrow last]
set ballGatherR [./dumperView create line 55 240 121 255 \
	-fill "#000000" -width 4 -arrow last]

# ---- X Scale ------------
scale ./joyStickX -label {joyX} -variable {joyX} -from {0} -to {255} \
  -orient horizontal
place ./joyStickX -x [expr $posJoysX + 0] -y [expr $posJoysY + 0]
set posJoysX [expr $posJoysX + 100]

# ---- Y Scale ------------
scale ./joyStickY -label {joyY} -variable {joyY} -from {255} -to {0} \
  -orient vertical
place ./joyStickY -x [expr $posJoysX] -y [expr $posJoysY + 0]
set posJoysX [expr $posJoysX + 80]

# ---- Pot1 ------------
scale ./pot1 -label {pot1} -variable {pot1} -from {255} -to {0} \
  -orient vertical
place ./pot1 -x [expr $posJoysX] -y [expr $posJoysY + 0]
set posJoysX [expr $posJoysX + 80]

# ---- Red1 LED ------------
button ./red1Button -padx $padX -pady $padY -borderwidth 3 -text "R1"
place ./red1Button -x [expr $posJoysX] -y [expr $posJoysY + 0]

# ---- Sw1 checkbutton ------------
checkbutton ./sw1Checkbutton -padx $padX -pady $padY -borderwidth 3 \
  -background {#c0c0c0} -activebackground {#f0f0f0} \
  -text "SW1" -variable {sw1} -offvalue 0 -onvalue 1
place ./sw1Checkbutton -x [expr $posJoysX - 20] -y [expr $posJoysY + 40]
set posJoysX [expr $posJoysX + 55]

# ---- Green1 LED ------------
button ./green1Button -padx $padX -pady $padY -borderwidth 3 -text "G1"
place ./green1Button -x [expr $posJoysX] -y [expr $posJoysY + 0]

# ---- Pb1 checkbutton ------------
checkbutton ./pb1Checkbutton -padx $padX -pady $padY -borderwidth 3 \
  -background {#c0c0c0} -activebackground {#f0f0f0} \
  -text "PB1" -variable {pb1} -offvalue 0 -onvalue 2
place ./pb1Checkbutton -x [expr $posJoysX - 20] -y [expr $posJoysY + 40]
set posJoysX [expr $posJoysX + 55]

# ---- Pb2 checkbutton ------------
checkbutton ./pb2Checkbutton -padx $padX -pady $padY -borderwidth 3 \
  -background {#c0c0c0} -activebackground {#f0f0f0} \
  -text "PB2" -variable {pb2} -offvalue 0 -onvalue 4
place ./pb2Checkbutton -x [expr $posJoysX - 20] -y [expr $posJoysY + 40]
set posJoysX [expr $posJoysX + 55]

set posJoysX 20
set posJoysY [expr $posJoysY + 100]


# ---- Pot2 ------------
scale ./pot2 -label {pot2} -variable {pot2} -from {255} -to {0} \
  -orient vertical
place ./pot2 -x [expr $posJoysX] -y [expr $posJoysY + 0]
set posJoysX [expr $posJoysX + 80]

# ---- Pb3 checkbutton ------------
checkbutton ./pb3Checkbutton -padx $padX -pady $padY -borderwidth 3 \
  -background {#c0c0c0} -activebackground {#f0f0f0} \
  -text "PB3" -variable {pb3} -offvalue 0 -onvalue 32
place ./pb3Checkbutton -x [expr $posJoysX - 20] -y [expr $posJoysY + 40]
set posJoysX [expr $posJoysX + 55]

# ---- Pb4 checkbutton ------------
checkbutton ./pb4Checkbutton -padx $padX -pady $padY -borderwidth 3 \
  -background {#c0c0c0} -activebackground {#f0f0f0} \
  -text "PB4" -variable {pb4} -offvalue 0 -onvalue 64
place ./pb4Checkbutton -x [expr $posJoysX - 20] -y [expr $posJoysY + 40]
set posJoysX [expr $posJoysX + 55]

# ---- Sw2 checkbutton ------------
checkbutton ./sw2Checkbutton -padx $padX -pady $padY -borderwidth 3 \
  -background {#c0c0c0} -activebackground {#f0f0f0} \
  -text "SW2" -variable {sw2} -offvalue 0 -onvalue 128
place ./sw2Checkbutton -x [expr $posJoysX - 20] -y [expr $posJoysY + 40]
set posJoysX [expr $posJoysX + 55]

# ---- Red2 LED ------------
button ./red2Button -padx $padX -pady $padY -borderwidth 3 -text "R2"
place ./red2Button -x [expr $posJoysX] -y [expr $posJoysY + 0]

# ---- Sw3 checkbutton ------------
checkbutton ./sw3Checkbutton -padx $padX -pady $padY -borderwidth 3 \
  -background {#c0c0c0} -activebackground {#f0f0f0} \
  -text "SW3" -variable {sw3} -offvalue 0 -onvalue 16
place ./sw3Checkbutton -x [expr $posJoysX - 20] -y [expr $posJoysY + 40]
set posJoysX [expr $posJoysX + 55]

# ---- Sw_JoyT checkbutton ------------
checkbutton ./sw_joyTCheckbutton -padx $padX -pady $padY -borderwidth 3 \
  -background {#c0c0c0} -activebackground {#f0f0f0} \
  -text "SW_JOYT" -variable {p2_sw_trig} -offvalue 0 -onvalue 1
place ./sw_joyTCheckbutton -x [expr $posJoysX - 20] -y [expr $posJoysY + 40]
set posJoysX [expr $posJoysX + 55]

# ---- Green2 LED ------------
button ./green2Button -padx $padX -pady $padY -borderwidth 3 -text "G2"
place ./green2Button -x [expr $posJoysX] -y [expr $posJoysY + 0]

## ---- Sw4 checkbutton ------------
#checkbutton ./sw4Checkbutton -padx $padX -pady $padY -borderwidth 3 \
#  -background {#c0c0c0} -activebackground {#f0f0f0} \
#  -text "SW4" -variable {sw4} -offvalue 0 -onvalue 0
#place ./sw4Checkbutton -x [expr $posJoysX - 20] -y [expr $posJoysY + 40]
#set posJoysX [expr $posJoysX + 55]

## ---- Sw5 checkbutton ------------
#checkbutton ./sw5Checkbutton -padx $padX -pady $padY -borderwidth 3 \
#  -background {#c0c0c0} -activebackground {#f0f0f0} \
#  -text "SW5" -variable {sw5} -offvalue 0 -onvalue 0
#place ./sw5Checkbutton -x [expr $posJoysX - 20] -y [expr $posJoysY + 40]
#set posJoysX [expr $posJoysX + 55]

# ---- Run button ------------
button ./runButton -padx $padX -pady $padY -borderwidth 3 -text "RUN" \
  -command runButtonEvent
place ./runButton -x $posButtonsX -y [expr $posButtonsY + 0]
set posButtonsX [expr $posButtonsX + 55]

# ---- Stop button ------------
button ./stopButton -padx $padX -pady $padY -borderwidth 3 -text "STOP" \
  -command stopButtonEvent
place ./stopButton -x $posButtonsX -y [expr $posButtonsY + 0]
set posButtonsX [expr $posButtonsX + 62]

# ---- Step button ------------
button ./stepButton -padx $padX -pady $padY -borderwidth 3 -text "STEP" \
  -command stepButtonEvent
place ./stepButton -x $posButtonsX -y [expr $posButtonsY + 0]
set posButtonsX [expr $posButtonsX + 65]

# ---- Exit button ------------
button ./exitButton -padx $padX -pady $padY -borderwidth 3 -text "EXIT" \
	-command exitButtonEvent \
	-background {#cf0000} -activebackground {#ef0000}
place ./exitButton -x [expr $posButtonsX +20] -y [expr $posButtonsY + 0]
set posButtonsX [expr $posButtonsX + 73]

# ---- Clock Rate ------------
scale ./clockRate -variable {clock} -from {1.0} -to {50.0} \
  -orient horizontal
place ./clockRate -x 20 -y [expr $posButtonsY + 30]

set logfileID [open "logfile" w]
set pipeID [open "|$executable" w+]
updatePos
# ----------------------------------------------