/*
  micromouse action algorithm simulator
  Hirotaka JOE Ohkubo 2001
    for WildC++ with tcl/tk 8.3

  is based on :

# Map Editor for Micro Mouse
# (c) Copyright 1996, T.Yasui
#	1996/05/03 V0.0a Test Version

*/

/********************** Map Editor Part **********************/

/* coodination

     0 1 2 3 4 5 .. x
   0 S     W
   1       |
   2  S ---+-\ N
   .       |
   .       E        _ �� bit 0 of Map[x][y]
   y               |  �� bit 1 of Map[x][y]
*/

#define MOUSESIM

static int mousesim_Map[17][17];
static int mousesim_WallIdNS[17][17];
static int mousesim_WallIdEW[17][17];

int mousesim_readMap(int x, int y) {
  return mousesim_Map[x][y];
}

void mousesim_writeMap(int x, int y, int v) {
  mousesim_Map[x][y] = v;
}

int mousesim_readWallIdNS(int x, int y) {
  return mousesim_WallIdNS[x][y];
}

int mousesim_readWallIdEW(int x, int y) {
  return mousesim_WallIdEW[x][y];
}

void mousesim_setWallIdNS(int x, int y, int id) {
  mousesim_WallIdNS[x][y] = id;
}

void mousesim_setWallIdEW(int x, int y, int id) {
  mousesim_WallIdEW[x][y] = id;
}

void mousesim_initMapArray() {
  int x,y;
  for (x=0;x<=16;x++) {
    for (y=0;y<=16;y++) {
      mousesim_Map[x][y] = 0;
    }
  }
  for (x=1;x<=15;x++) {
    mousesim_Map[x][0] = 1;
    mousesim_Map[x][16] = 1;
    mousesim_Map[0][x] = 2;
    mousesim_Map[16][x] = 2;
  }
  mousesim_Map[0][0] = 3;
  mousesim_Map[0][16] = 1;
  mousesim_Map[16][0] = 2;
  mousesim_Map[16][16] = 0;
  mousesim_Map[0][1] = 3;
} //initMapArray

void MousePuts(char *msg) {
#pragma tcl interp msg
  global msg ;
  .base.p.m.message config -state normal ;
  .base.p.m.message insert end $msg ;
  .base.p.m.message config -state disabled ;
  .base.p.m.message yview end ;
#pragma endtcl
} // MousePuts

/************************ Mouse Simultor Part **********************/

char mousesim_userprog[256];

enum Action { Step, TurnLeft, TurnRight, TurnBack, End };

void mousesim_LoadProgram(char *filename) {
  if (mousesim_userprog[0] != '\0') {  G__unloadfile(mousesim_userprog); }
  strcpy(mousesim_userprog,filename);
  G__loadfile(mousesim_userprog);
  mousesim_ResetSimulation();
} // LoadProgram

void mousesim_ReloadProgram() {
  G__unloadfile(mousesim_userprog);
  G__loadfile(mousesim_userprog);
  mousesim_ResetSimulation();
}
  
void mousesim_ResetSimulation() {
  mousesim_theMouseStatus.InitMouse();
  MouseInit();
}

class mousesim_MouseStatus {
  int x,y,dir ;
  int step,walk,turn,uturn;
  void ShowMouse();
  int rotx(int dx, int dy);
  int roty(int dx, int dy);
  public:
  int ready;
  void InitMouse();
  int sense(int dir);
  void doAction();
  void Bring2Start();
} ;

void mousesim_MouseStatus::InitMouse () {
  x = 0;
  y = 0;
  dir = 0;
  ready = 1;
  step = 0;
  walk = 0;
  turn = 0;
  uturn = 0;

  ShowMouse();
}

int mousesim_MouseStatus::rotx(int dx, int dy) {
  switch (dir) {
    case 0: return dx;
    case 1: return dy;
    case 2: return -dx;
    case 3: return -dy;
  }
}

int mousesim_MouseStatus::roty(int dx, int dy) {
  switch (dir) {
    case 0: return dy;
    case 1: return dx;
    case 2: return -dy;
    case 3: return -dx;
  }
}

void mousesim_MouseStatus::ShowMouse() {
  char msg[256], *msgp;
  int x1,y1,x2,y2,x3,y3;

  sprintf(msg,"%d steps\n%d walks\n%d turns\n%d Uturn",step,walk,turn,uturn);
  msgp = msg;

#pragma tcl interp msgp
global msgp ;
.base.p.st config -state normal ;
.base.p.st delete "0.0" end ;
.base.p.st insert end $msgp mytag ;
.base.p.st tag configure mytag -justify right ;
.base.p.st config -state disabled ;
#pragma endtcl

  int xc = 27+25*x;
  int yc = 27+25*y;

  x1 = xc+rotx( 7, 0); y1 = yc+roty( 7, 0);
  x2 = xc+rotx(-6, 5); y2 = yc+roty(-6, 5);
  x3 = xc+rotx(-6,-5); y3 = yc+roty(-6,-5); 

#pragma tcl interp x1 y1 x2 y2 x3 y3
global x1 y1 x2 y2 x3 y3 ;
.base.c delete mouse ;
.base.c create polygon $x1 $y1 $x2 $y2 $x3 $y3 -fill Blue -outline black -tags mouse ;
#pragma endtcl

}

int senseForward() { return mousesim_theMouseStatus.sense(0); }
int senseLeft()    { return mousesim_theMouseStatus.sense(-1); }
int senseRight()   { return mousesim_theMouseStatus.sense(1); }

int mousesim_MouseStatus::sense(int d) {
  int direction = (dir+d+4) % 4;
  switch (direction) {
  case 0: return mousesim_Map[x+1][y] & 2;
  case 1: return mousesim_Map[x][y+1] & 1;
  case 2: return mousesim_Map[x  ][y] & 2;
  case 3: return mousesim_Map[x  ][y] & 1;
  }
}

void mousesim_MouseStatus::doAction() {
  char msg[30], *msgp;

  if (!ready) { return; }

  Action res = MousedoAction();
  step++;
  sprintf(msg,"%3d: ",step);
  msgp = msg;
  MousePuts(msg);

  switch (res) {
    case Step:
      walk++;
      if (sense(0)) {
	MousePuts("Step...Crash!!!\n");
	ready = 0;
	break;
      }
      MousePuts("Step\n");
      switch (dir) {
      case 0: x++; break;
      case 1: y++; break;
      case 2: x--; break;
      case 3: y--; break;
      }
      ShowMouse();
      break;
    case TurnLeft:
      MousePuts("Left\n");
      dir = (dir + 3) % 4;
      turn++;
      ShowMouse();
      break;
    case TurnRight:
      MousePuts("Right\n");
      dir = (dir + 1) % 4;
      turn++;
      ShowMouse();
      break;
    case TurnBack:
      MousePuts("U-Turn\n");
      dir = (dir + 2) % 4;
      uturn++;
      ShowMouse();
      break;
    case End:
      MousePuts("End\n");
      ready = 0;
      break;
  }
}

void mousesim_MouseStatus::Bring2Start() {
  x = 0;
  y = 0;
  dir = 0;
  ready = 1;
  MouseBring2Start();
  ShowMouse();
}

mousesim_MouseStatus mousesim_theMouseStatus;

main() {
  mousesim_initMapArray();
  mousesim_userprog[0]='\0';

#pragma tcl interp

########################## Map Editor Part #########################

# constant
set x_base 15
set y_base 15
set delta 25

proc MakeMenuBar {} {
  frame .menubar -relief raised -borderwidth 1 ;
  pack .menubar -fill x ;
  menubutton .menubar.f -text "File" -menu .menubar.f.m ;
  menubutton .menubar.p -text "Simulation" -menu .menubar.p.m ;
  menubutton .menubar.u -text "UserCmd" -menu .menubar.u.m ;
  menubutton .menubar.l -text "Log" -menu .menubar.l.m ;
  pack .menubar.f .menubar.p .menubar.u .menubar.l -side left ;
}
proc MakeFileMenu {} {
  menu .menubar.f.m ;
  .menubar.f.m add command -label "New Map" -command ClearMap ;
  .menubar.f.m add command -label "Load Map File" -command LoadMap ;
  .menubar.f.m add command -label "Save Map File" -command SaveMap ;
  .menubar.f.m add command -label "Name Map" -command NameMap ;
  .menubar.f.m add command -label "Export As Text" -command ExportText ;
  .menubar.f.m add command -label "Quit" -command "ceval WildCard_Exit()" ;
}
proc MakeLogMenu {} {
  menu .menubar.l.m ;
  .menubar.l.m add command -label "Save Log" -command SaveLog ;
  .menubar.l.m add command -label "Clear Log" -command ClearMessage ;
}
proc MakeUserMenu {} {
  menu .menubar.u.m ;
  .menubar.u.m add command -label "User1" -command "ceval MouseUserCmd(1)" ;
  .menubar.u.m add command -label "User2" -command "ceval MouseUserCmd(2)" ;
  .menubar.u.m add command -label "User3" -command "ceval MouseUserCmd(3)" ;
  .menubar.u.m add command -label "User4" -command "ceval MouseUserCmd(4)" ;
  .menubar.u.m add command -label "User5" -command "ceval MouseUserCmd(5)" ;
}
proc MakeProgMenu {} {
  menu .menubar.p.m ;
  .menubar.p.m add command -label "Load Program" -command LoadProgram ;
  .menubar.p.m add command -label "Reload Program" -command "ceval mousesim_ReloadProgram()" ;
  .menubar.p.m add command -label "Reset Simulation" -command "ceval mousesim_ResetSimulation()" ;
  .menubar.p.m add command -label "Bring to Start" -command "ceval mousesim_theMouseStatus.Bring2Start()" ;
}
proc MakeMain {} {
  frame .base ;
  pack .base ;
  canvas .base.c -width 425 -height 425 -closeenough 5.0 ;
  frame .base.p ;
  pack .base.c .base.p -side left ;
}
proc MakeControls {} {
  label .base.p.title -text "Untitled" ;
  frame .base.p.b ;
  text .base.p.st -width 22 -height 4 -font {Courier 9} ;
  frame .base.p.m ;
  pack .base.p.title .base.p.b .base.p.st .base.p.m -pady 2 ;
  
  button .base.p.b.run -text "Run/Stop" -command "intervalAction 1" ;
  button .base.p.b.step -text "Step" -command "after idle ceval mousesim_theMouseStatus.doAction()" ;
  pack .base.p.b.run .base.p.b.step -side left ;

  text .base.p.m.message -width 20 -state disabled -yscrollcommand ".base.p.m.scr set" -wrap char -height 22 -font {Courier 9} ;
  pack .base.p.m.message -side left ;
  scrollbar .base.p.m.scr -command ".base.p.m.message yview" ;
  pack .base.p.m.scr -side left -fill y ;
}

proc InitMap {} {
  DrawIndexes ;
  DrawPoles ;
  DrawWalls ;
  SyncWalls ;
}
# InitMap
proc DrawIndexes {} {
  global x_base y_base delta ;
  set x1 [expr $x_base+10] ;
  set y1 [expr $y_base+12] ;
  set x9 [expr $x_base-9] ;
  set y8 [expr $y_base-8] ;
  for {set x 0} {$x <= 15} {incr x} {
    .base.c create text $x1 $y8 -text $x ;
    set x1 [expr $x1+$delta] ;
    .base.c create text $x9 $y1 -text $x ;
    set y1 [expr $y1+$delta] ;
  } ;

  set x1 [expr $x_base+12];
  set y1 [expr $y_base+12];
  .base.c create text [expr $x1] [expr $y1] -text S -fill brown1;
  set x1 [expr $x1+$delta*7];
  set x2 [expr $x1+$delta];
  set y1 [expr $y1+$delta*7];
  set y2 [expr $y1+$delta];
  .base.c create text $x1 $y1 -text G -fill PaleGreen4 ;
  .base.c create text $x2 $y1 -text O -fill PaleGreen4 ;
  .base.c create text $x1 $y2 -text A -fill PaleGreen4 ;
  .base.c create text $x2 $y2 -text L -fill PaleGreen4 ;
}
# DrawIndexes
proc DrawPoles {} {
  global x_base y_base delta ;

  set limit [expr $x_base + $delta * 16] ;
  for {set x [expr $x_base-2]} {$x < $limit} {set x [expr $x+$delta]} {
    for {set y [expr $y_base-2]} {$y < $limit} {set y [expr $y+$delta]} {
      .base.c create oval $x $y [expr $x+4] [expr $y+4] -outline black -fill red ;
    } ;
  } ;
}
# DrawPoles
proc DrawWalls {} {
  global x_base y_base delta ;
  for {set x 0} {$x <=16} {incr x} {
    for {set y 0} {$y <=16} {incr y} {
      set x1 [expr $x_base+$x*$delta] ;
      set y1 [expr $y_base+$y*$delta] ;
      if {$x != 16} {
	set id [.base.c create line [expr $x1+5] $y1 [expr $x1+20] $y1 -width 3 ];
	if { ($y != 0) && ($y != 16) } {
	  .base.c bind $id <Button> "ClickWall $id $x $y 1" ;
	} ;
	ceval "mousesim_setWallIdNS($x,$y,$id)" ;
      } ;
      if {$y != 16} {
	set id [.base.c create line $x1 [expr $y1+5] $x1 [expr $y1+20] -width 3 ];
	if { ($x != 0) && ($x != 16) } {
	  .base.c bind $id <Button> "ClickWall $id $x $y 2" ;
	} ;
	ceval "mousesim_setWallIdEW" ($x,$y,$id) ;
      } ;
    } ;
  } ;
} ;
# DrawWalls

proc SyncWalls {} {
  for {set x 0} {$x <= 16} {incr x} {
    for {set y 0} {$y <= 16} {incr y} {
      set w [ceval "mousesim_readMap" ($x,$y)] ;
      set ns [ceval "mousesim_readWallIdNS" ($x,$y)] ;
      if {$ns != 0} {
	.base.c itemconfig $ns -fill [expr ($w & 1)?"black":""];
      } ;
      set ew [ceval "mousesim_readWallIdEW" ($x,$y)] ;
      if {$ew != 0} {
	.base.c itemconfig $ew -fill [expr ($w & 2)?"black":""];
      }
    }
  }
}
# SyncWalls

proc ClickWall {id x y b} {
  set m [ceval "mousesim_readMap" ($x,$y) ] ;
  if {$m & $b} {
    .base.c itemconfigure $id -fill "";
  } else {
    .base.c itemconfigure $id -fill black;
  } ;
  set m [expr $m ^ $b];
  ceval "mousesim_writeMap" ($x,$y,$m) ;  
}
# ClickWall

proc ClearMap {} {
  ceval "mousesim_initMapArray" () ;
  SyncWalls ;
}
# ClearMap

proc LoadMap {} {
  set filename [tk_getOpenFile -filetypes {{{TYmapedit Files} {.map}}}] ;
  if {$filename == ""} { return ; } ;

  ceval "mousesim_initMapArray" () ;

  set fd [open $filename r] ;

  gets $fd line ;
  set title [string range $line 2 end] ;

  for {set y 0} {$y <=15} {incr y} {
    gets $fd line ;
    for {set x 0} {$x <=15} {incr x} {
      ceval "mousesim_writeMap" ($x,$y,[string index $line $x]) ;
    } ;
  } ;

  close $fd ;
  SyncWalls ;
  .base.p.title config -text $title ;
}
# LoadMap

proc SaveMap {} {
  set filename [tk_getSaveFile -filetypes {{{TYmapedit Files} {.map}}}] ;
  if {$filename == ""} { return } ;

  set title [.base.p.title cget -text] ;

  set fd [open $filename w] ;
  puts $fd "; $title" ;
  for {set y 0} {$y <=15} {incr y} {
    for {set x 0} {$x <=15} {incr x} {
      puts -nonewline $fd [ceval "mousesim_readMap" ($x,$y)] ;
    } ;
    puts $fd [format " "] ;
  } ;
  close $fd ;
}
# SaveMap

proc NameMap {} {
  global newtitle ;
  toplevel .dialog ;
  wm title .dialog "Naming Map" ;
  label .dialog.label -text "Map title:" ;
  set newtitle [.base.p.title cget -text] ;
  entry .dialog.entry -relief sunken -textvariable newtitle ;
  pack .dialog.label .dialog.entry -side left -padx 1m -pady 1m ;
  bind .dialog.entry <Return> {
    global newtitle ;
    .base.p.title config -text $newtitle ;
    destroy .dialog ;
  } ;
  grab set .dialog ;
} ;
# NameMap

proc ExportText {} {
  set filename [tk_getSaveFile -filetypes {{{Text Files} {.txt}}}] ;
  if {$filename == ""} { return } ;

  set title [.base.p.title cget -text] ;

  set fd [open $filename w] ;
  puts $fd "Title = $title" ;
  puts $fd "   0 1 2 3 4 5 6 7 8 9 A B C D E F" ;
  for {set y 0} {$y < 16} {incr y} {
    puts -nonewline $fd "  +" ;
    set south [format "%2d" $y] ;
    for {set x 0} {$x < 16} {incr x} {
      set m [ceval "mousesim_readMap" ($x,$y)] ;
      if {[expr $m & 1]} {
	puts -nonewline $fd "-+" ;
      } else {
	puts -nonewline $fd " +" ;
      } ;
      set s [computeSouth $m $x $y] ;
      set south "$south$s" ;
    } ;
    puts $fd "";
    puts $fd "${south}|" ;
  } ;
  puts $fd "  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+" ;
  close $fd ;
}
# ExportText

proc computeSouth {m x y} {
  if {[expr $m & 2]} {
    set s "|" ;
  } else {
    set s " " ;
  } ;
        if {($x==0)&&($y==0)} {
    set s "${s}S" ;
  } elseif {($x==7)&&($y==7)} {
    set s "${s}G" ;
  } elseif {($x==8)&&($y==7)} {
    set s "${s}O" ;
  } elseif {($x==7)&&($y==8)} {
    set s "${s}A" ;
  } elseif {($x==8)&&($y==8)} {
    set s "${s}L" ;
  } else {
    set s "${s} " ;
  } ;

  return $s ;
}
# computeSouth

proc ClearMessage {} {
  .base.p.m.message config -state normal ;
  .base.p.m.message delete "0.0" end ;
  .base.p.m.message config -state disabled ;
}

proc SaveLog {} {
  set filename [tk_getSaveFile -filetypes {{{Text Files} {.txt}}}] ;
  if {$filename == ""} { return } ;

  set fd [open $filename w] ;
  puts -nonewline $fd [.base.p.m.message get "0.0" end] ;
  close $fd ;
}
# SaveLog

############################# Mouse Simulator Part ##################

proc LoadProgram {} {
  set filename [tk_getOpenFile -filetypes {{{C Source Files} {.c}}}] ;
  if {$filename == ""} { return ; } ;

  ceval "mousesim_LoadProgram" (\"$filename\") ;
}
# LoadProgram

# interval action

set cronid no ;

proc intervalAction {push} {
  global cronid ;
  if {$push} {
    if {0 != [string compare $cronid no]} {
      after cancel $cronid ;
      set cronid no ;
      return ;
    } ;
  } ;

  ceval "mousesim_theMouseStatus.doAction" () ;

  set r [ceval "mousesim_theMouseStatus.ready"] ;
  if {$r} {set cronid [after 200 "intervalAction 0"]} else {set cronid no} ;
}

####### application setup

wm title . "Micromouse Simulator" ;
MakeMenuBar ;
MakeFileMenu ; MakeLogMenu ; MakeUserMenu ; MakeProgMenu ;
MakeMain ;
MakeControls ;
InitMap ;

#pragma endtcl

  mousesim_theMouseStatus.InitMouse();
  WildCard_MainLoop();

}