package JBot;

import java.util.* ;
import stamp.util.os.* ;

/**
 * 単純な障害物回避タスク
 * 
 * センサーオブジェクトを用いて物体から離れるようにする
 * J-Botは固定増加分移動する
 * 前に障害物に遭遇した場合、J-Botはランダムな方向に移動する。
 */

public class RandomWalkTask extends Task {
  JBotInterface jbot = new RampingJBot ( new MultitaskingJBot ()) ;
  BaseSensor sensor ;
  Random random = new Random () ;

  private boolean flipCoin () {
    return random.next() < ( Random.MAX_RAND / 2 ) ;
  }


  public RandomWalkTask ( BaseSensor sensor ) {
    this.sensor = sensor ;
  }

  protected void execute () {
    final int turnAround = 1 ;

    switch ( state ) {
    case initialState:
      // 多数のステップが要求された時にのみ状態変化が起こる。

      if ( sensor.obstacleDetected ()) {
        int direction = sensor.obstacleDirection () ;

        if ( direction < 75 ) {
          // 左に何かがある
          jbot.pivot ( -2 ) ;
        } else
        if ( direction > 105 ) {
          // 右に何かがある
          jbot.pivot ( 2 ) ;
        } else {
          // 前に障害物がある

          if ( flipCoin ()) {
            // 後退し、他の事をする
            jbot.move ( -2 ) ;
            jbot.wait ( turnAround ) ;
          } else {
            // 左か右に回転することを試みる
            if ( flipCoin ()) {
              jbot.pivot ( -2 ) ;
            } else {
              jbot.pivot ( 2 ) ;
            }
          }
        }
      } else {
        // 何も検出されないので前進する。

        jbot.move (jbot.continuousForward) ;
      }
      break;

    case turnAround:
      // J-Botは後退し、障害物から離れる

      if ( flipCoin ()) {
        // 180゜回転する
        jbot.pivot ( 4 ) ;
      } else if ( flipCoin ()) {
        // 右に90゜回転する
        jbot.pivot ( -2 ) ;
      } else {
        // 左に90゜回転する
        jbot.pivot ( 2 ) ;
      }

      // 初期ステータスチェックのため後ろに戻る
      jbot.wait ( initialState ) ;
      break;

    default:                    // おかしなステータスを捕獲する
      stop () ;
      break;
    }
  }
}

import JBot.*;
import java.util.* ;
import stamp.core.*;
import stamp.util.os.* ;

/**
 * Random Walkを用いた迷路テストプログラム
 * 
 * ランダムな動作を用いて迷路を脱出しようと試みる
 */

public class RandomWalkTest1 {

  static public void main () {
    new RandomWalkTask
          ( new IrRangeSensor // IrRangeDropOffSensor
                        ( CPU.pin7    // left DAC pin
                        , CPU.pin1    // left PWM pin
                        , CPU.pin0    // left detector pin
                        , CPU.pin8    // right DAC pin
                        , CPU.pin3    // right PWM pin
                        , CPU.pin2    // right detector pin
                        , 3           // deadband
                        )) ;

    try {
      Task.TaskManager () ;
    } catch ( Exception e ) {
      System.out.println ( "Error detected" ) ;
    } finally {
      System.out.println ( "All done" ) ;
    }
  }
}

package JBot;

import java.util.* ;
import stamp.util.os.* ;

/**
 * right hand ruleを用いた迷路探索タスク 
 * 
 * 右側の壁を伝わって迷路を出ようとする。
 * 一番初めにJ-Botは壁を検出するまで直進する。
 * それがJ-Botの右側になるように回転する。
 * 前方に障害物が無いと仮定して短距離直進する。
 * 壁がまだそこにあることを確かめるために右に回転する。
 * これはセンサーがJ-Botのすぐ右に壁があることを検出できないため
 * 必要である。前進して右を見るときだけ壁を検出できる。もし壁がまだ
 * 側にあれば、J-Botは左に回転し前進を続ける。
 * この過程は右回転で壁が検出されないか、壁が前にありJ-Botが左に回転
 * するまで繰り返される。
 * このプログラムが走っている迷路は厳密な方法でセンサーを用いている。
 * すなわち、J-Botが新しい障害物チェックに反応する自由動作に対する
 * 動きの前に、センサーが正しい結果を出すまで待ちます。
 */

public class RightHandRuleTask extends Task {
  JBotInterface jbot = new WheelEncoderJBot ( new MultitaskingJBot ()) ;
  BaseSensor sensor ;

  /**
   * 新しいタスクを作る
   *
   * 入力: BaseSensor sensor: 用いるセンサー
   */
  public RightHandRuleTask ( BaseSensor sensor ) {
    this.sensor = sensor ;
    sensor.setEvent ( this ) ;
    jbot.setNextEvent ( this ) ;
  }


  /**
   * 次の状態を設定する。タスクを停止する。
   * 通知された時に提示された状態でタスクがリスタートする。
   */
  protected void waitNextState ( int nextState ) {
    nextState(nextState) ;
    suspend() ;
  }


  /**
   * センサーを開始する。次の状態を設定する。タスクを停止する。
   * 通知された時に提示された状態でタスクがリスタートする。
   *
   * @param nextState next task state to be execute when sensor is done
   */
  protected void waitSensorNextState ( int nextState ) {
    waitNextState(nextState) ;
    sensor.obstacleDetected();
  }


  static final int checkForObstacle = 1 ;
  static final int doneWallToRight = 2 ;
  static final int checkAlongWall = 3 ;
  static final int turnToWallOnRight = 4 ;
  static final int checkWallOnRight = 5 ;
  static final int checkForWall = 6 ;

  /**
   * メインタスクルーチン
   * センサーあるいはJ-Botの動きのタスクを開始し、停止する。
   * 対応するセンサーあるいはタスクが完了した時にタスクが開始され、
   * このタスクをリスタートする。
   */
  protected void execute () {
    switch ( state ) {
    case initialState:
      // 障害物チェックセンサーを開始する
      waitSensorNextState(checkForObstacle);
      break;

    case checkForObstacle:
      // 障害物が検出されたかどうかチェックする
      if ( sensor.obstacleDetected()) {
        int direction = sensor.obstacleDirection();
        if ( direction < 75 ) {
          // 左に障害物あり、180゜左に旋回する。
          jbot.pivot(4) ;
        } else if ( direction > 135 ) {
          // 右に障害物あり、90゜左に旋回する。
          jbot.pivot(2) ;
        } else {
          // 前方に障害物あり。90゜左に旋回する。
          jbot.pivot(2) ;
        }

        // 動作が完了するまで待つ。
        waitNextState(doneWallToRight);
      } else {
        // 何も検出されない。１ステップ前進する。再度繰り返す。
        jbot.move(1) ;
        waitNextState(initialState);
      }
      break;

    case doneWallToRight:
      // J-Botは障害物から離れた。
      waitNextState( checkAlongWall ) ;
      sensor.obstacleDetected();
      break;

    case checkAlongWall:
      // 壁は右側にあるべき。
      // 前に何があるかチェックする
      if ( sensor.obstacleDetected ()) {
        int direction = sensor.obstacleDirection();
        if ( direction < 75 ) {
          // 左に障害物あり、多分角にいる。
          jbot.pivot(2) ;
        } else if ( direction > 135 ) {
          // 右に障害物あり、多分、端に近い。
          jbot.pivot(1) ;
        } else {
          // 前方に障害物がある。
          jbot.pivot(2) ;
        }
        waitNextState(doneWallToRight);
      } else {
        // 何も検出されない。１ステップ前進し壁をチェックする。

        jbot.move(1) ;
        waitNextState(turnToWallOnRight);
      }
      break;

    case turnToWallOnRight:
      // 前方に移動する。右側に壁があることをチェックする。
      jbot.pivot(-2);
      waitNextState(checkWallOnRight);
      break;

    case checkWallOnRight:
      // J-Botは前方に移動した。
      // 右側にまだ壁があるかどうか知るためにセンサーを用いる。
      waitSensorNextState(checkForWall) ;
      break;

    case checkForWall:
      // 右側に壁がある。
      // J-Botは壁に今直面する。
      // 障害物の状況により移動する。
      if ( sensor.obstacleDetected ()) {
        // 壁はまだそこにある。左に後ろに回る
        jbot.pivot(2);
        waitNextState(doneWallToRight);
      } else {
        // 前方になにもない。壁は右にあるだろう。
        jbot.move(1);
        waitNextState(initialState);
      }

      break;

    default:                    // defaultはおかしな状態を捕捉する。
      stop () ;
      break;
    }
  }
}

package JBot;

import java.util.* ;
import stamp.core.*;
import stamp.util.os.* ;

/**
 * 右手の法則での迷路テストプログラム
 */

public class RightHandRuleTest1 {

  static public void main () {
    new RightHandRuleTask
          ( new IrRangeSensor // IrRangeDropOffSensor
                        ( CPU.pin7    // left DAC pin
                        , CPU.pin1    // left PWM pin
                        , CPU.pin0    // left detector pin
                        , CPU.pin8    // right DAC pin
                        , CPU.pin3    // right PWM pin
                        , CPU.pin2    // right detector pin
                        , 3           // deadband
                        )) ;

    try {
      Task.TaskManager () ;
    } catch ( Exception e ) {
      System.out.println ( "Error detected" ) ;
    } finally {
      System.out.println ( "All done" ) ;
    }
  }
}

import stamp.core.*;
import stamp.util.os.* ;
import JBot.* ;

/**
 * MapMazeTaskクラスをテストする
 * 
 * J-Botは地図を作っている間迷路を探索する
 */

public class MazeMapTaskTest1 {
  public static void main () {
    int buzzerPin = CPU.pin4 ;
    FREQOUT buzzer ;

    printableMazeMap map = new printableMazeMap ( 20, 20 ) ;
    new MapMazeTask
      ( new IrRangeSensor ( CPU.pin7    // left DAC pin
                          , CPU.pin1    // left PWM pin
                          , CPU.pin0    // left detector pin
                          , CPU.pin8    // right DAC pin
                          , CPU.pin3    // right PWM pin
                          , CPU.pin2    // right detector pin
                          , 1           // deadband
                          )
      , 5           // 3-5cmの検出に対する範囲値
      , new RampingJBot ( new MultitaskingJBot ())
      , map ) ;

    // タスクを走らせる
    Task.TaskManager () ;

    // EEPROMに地図を書き込む
    map.save();

    // ブザーを鳴らす
    buzzer = new FREQOUT ( buzzerPin ) ;
    buzzer.freqout ( 100, 100 ) ;
    CPU.delay ( 10000 ) ;
    buzzer.stop();

    System.out.println ( "All done" ) ;
  }
}

import stamp.core.*;
import JBot.*;

/**
 * MazeMapTaskTest1の地図をダンプする
 */

public class DumpMazeMapTaskTest1  {
  public static void main() {
    printableMazeMap map = printableMazeMap.load() ;

    map.print("Stored map");
  }
}

package JBot;

import stamp.util.os.* ;
import stamp.core.*;

/**
 * 障害物と壁を避けながら迷路を探索する
 * 
 * printableMazeMapで迷路の状況を保存する
 * タスクは探索する場所が無くなった時に停止する
 * センサーの範囲は約3cmと仮定する
 * J-Botは15cm四方内に入っているとする
 * 各々の地図のマス目は15×15cm四方である
 * 任意の障害物はJ-Botの前方にあると仮定する
 * これは地図作りと意思決定を簡単にする
 * このプログラムはバックトラックをしたときに障害物をチェックしない
 */

public class MapMazeTask extends Task {
  protected JBotInterface    jbot ;
  protected BaseSensor       sensor ;
  protected int              obstacleRange ;
  protected printableMazeMap map ;

  protected char facing ;
  protected char backtrackPath[];
  protected int  pathIndex ;
  protected int  pathSize ;

  /**
   * 迷路の地図を作るタスクを作る
   *
   * 入力:BaseSensor sensor: 障害物をチェックするセンサー
   * 入力:int obstacleRange: 3-5cmに対する検出範囲
   * 入力:JBotInterface jbot: JBotを動かすため
   * 入力:printableMazeMap: map: 迷路を地図化する文字地図
   */
  public MapMazeTask ( BaseSensor sensor
                     , int        obstacleRange
                     , JBotInterface jbot
                     , printableMazeMap map ) {
    this.sensor = sensor ;
    this.obstacleRange = obstacleRange ;
    this.jbot = jbot ;
    this.map = map ;
    backtrackPath = new char [ 128 ] ;

    // デフォールトの方向を設定する
    facing = mazeMap.north ;
  }

  static protected final int stepDistance = 5 ; // cm 

  static protected final int computePath = 1 ;
  static protected final int followPath = 2 ;

  protected int checkingOffset = 0 ;

  /**
   * タスク状態ハンドラー
   */
  protected void execute () {
    switch ( state ) {
    case initialState:
      if ( checkingOffset == 6 ) {
        // 通過したマスをマークする
        map.move(facing);
        map.markTraveled();

        checkingOffset = 0 ;
      }

      // 前方に何があるか見る
      if ( sensor.ready()) {
        // センサーが読まれ、チックする
        if (    sensor.obstacleDetected ()
             && (    sensor.obstacleDistance(sensor.obstacleDirection())
                  <= obstacleRange)) {
          // 前方に障害物
          if ( checkingOffset == 0 ) {
            // 障害物が直前にある
            nextState(computePath);
          } else {
            // 障害物が前方5cmにある
            // 論理的地図のマスの中心をバックアップする
            jbot.move(-checkingOffset);
            jbot.wait(computePath);
            checkingOffset = 0 ;
          }
        } else {
          // センサーの範囲内で前方に移動する
          checkingOffset += stepDistance ;
          jbot.move(stepDistance);
          jbot.wait(initialState);
        }
      }
      break ;

    case computePath:
      // J-Botは現在のマス位置の中心にいる
      // 遭遇した障害物をマークする
      map.markObstacle(facing);

      //map.print("map");
      //map.printPath();

      // 新しい領域の探索のための道のりを計算する
      pathSize = map.computeBacktrackPath(backtrackPath) ;
      if ( 0 == pathSize ) {
// 探索が全て完了した。ブザーを鳴らし、地図をプリントするのを待つ。
        jbot.stop();
        stop();
        break;
      }

      // backtrackPathとpathSizeで指定された道のりに従う
      pathIndex = 0 ;
      nextState(followPath);

    case followPath:
      // その道のりで次のステップに従う
      if ( facing == backtrackPath[pathIndex]) {
        // その道のりに沿って移動する
        // 注意: 障害物に対するチェックはされない(要修正)
        // バックトラックパスのインデックスを修正する
        ++pathIndex;
        --pathSize;

        if ( pathSize == 0 ) {
          // 最後の動作の方向に向く
          nextState(initialState);
        } else {
          // 地図の位置を補正する
          map.move(facing);
          map.markTraveled();

          // pathに沿ってJ-Botを移動する
          jbot.move(6);
          jbot.wait(followPath) ;
        }
      } else {
        // pathに従って右の方向に向く
        int dir = facing-backtrackPath[pathIndex] ;
        if ( dir == 3 ) {
          dir = -1 ;
        } else if ( dir == -3 ) {
          dir = 1 ;
        }
        jbot.pivot (dir*2) ;
        jbot.wait ( followPath ) ;

        // 論理的方向を補正する
        facing = backtrackPath[pathIndex] ;
      }
      break;

    default:                    // 異常な状態を捕獲する
      stop () ;
      break;
    }
  }
}

package JBot;

import stamp.core.*;

/**
 * 二次元文字配列
 */

public class charArray {
  protected char s[];
  protected int xSize ;
  protected int ySize ;

  /**
   * ２次元の文字配列を作る
   *
   * 入力:int ySize: Yのサイズ
   * 入力:int xSize: Xのサイズ
   */
  public charArray ( int ySize, int xSize ) {
    s = new char [xSize * ySize];
    this.xSize = xSize ;
    this.ySize = ySize ;
    clear();
  }

  /**
   * xのサイズを得る
   *
   * 戻り: xのサイズ
   */
  public int xSize(){
    return xSize;
  }

  /**
   * yのサイズを得る
   *
   * 戻り: yのサイズ
   */
  public int ySize(){
    return ySize;
  }

  /**
   * xサイズの最小境界を得る
   *
   * 戻り: 0
   */
  public int xLow(){
    return 0;
  }

  /**
   * yサイズの最小境界を得る
   *
   * 戻り: 0
   */
  public int yLow(){
    return 0;
  }

  /**
   * x添え字の最大を得る
   *
   * 戻り:x添え字の最大
   */
  public int xHigh(){
    return xSize-1;
  }

  /**
   * y添え字の最大を得る
   *
   * 戻り:y添え字の最大
   */
  public int yHigh(){
    return ySize-1;
  }


  /**
   * 配列をクリアする
   */
  public void clear(){
    setAll (0);
  }

  /**
   * 全ての配列を設定する
   *
   * 入力:int c: 全てに文字を入れる
   */
  public void setAll(int c){
    int size = xSize * ySize ;

    for ( int i = 0; i < size ; ++ i ) {
      s[i]=(char)c;
    }
  }

  /**
   * 配列の添え字を得る
   *
   * 入力:int y: 配列の添え字
   * 入力:int x: 配列の添え字
   *
   * 戻り:　配列の添え字
   */
  protected int getIndex ( int y, int x ) throws IndexOutOfBoundsException {
    if (( x >= xSize ) || ( y >= ySize )) {
      IndexOutOfBoundsException.throwIt() ;
    }

    return (y * xSize) + x ;
  }

  /**
   * 配列から文字を得る
   *
   * 入力:int y: 配列の添え字
   * 入力:int x: 配列の添え字
   *
   * @returns character from array
   */
  public int get ( int y, int x ) throws IndexOutOfBoundsException {
    return s[getIndex(y,x)] & 0xff ;
  }

  /**
   * 配列に文字をセットする
   *
   * 入力:int y: 配列の添え字
   * 入力:int x: 配列の添え字
   * 入力:int c: セットする文字
   */
  public void set ( int y, int x, int c ) throws IndexOutOfBoundsException {
    s[getIndex(y,x)] = (char) c ;
  }
}

package JBot;

import stamp.core.*;

/**
 * ２次元の文字地図
 * 配列の端に負の軸をラップする
 */

public class charMap extends charArray {
  protected int xLow ;
  protected int yLow ;
  protected int xHigh ;
  protected int yHigh ;

  /**
   * ２次元の文字地図を作る
   *
   * 入力:int ySize: Yのサイズ
   * 入力:int xSize: Xのサイズ
   */
  public charMap ( int ySize, int xSize ) {
    super(ySize,xSize);
    resetBounds();
  }

  /**
   * 下限と上限をリセットする
   */
  public void resetBounds(){
    xLow  = 0 ;
    yLow  = 0 ;
    xHigh = 0 ;
    yHigh = 0 ;
  }

  /**
   * xサイズの最小境界を得る
   *
   * 戻り: xLow
   */
  public int xLow(){
    return xLow;
  }

  /**
   * yサイズの最小境界を得る
   *
   * 戻り: yLow
   */
  public int yLow(){
    return yLow;
  }


  /**
   * x添え字の最大を得る
   *
   * 戻り:x添え字の最大
   */
  public int xHigh(){
    return xHigh;
  }

  /**
   * y添え字の最大を得る
   *
   * 戻り:y添え字の最大
   */
  public int yHigh(){
    return yHigh;
  }

  /**
   * 配列の添え字を得る
   *
   * 入力:int y: 配列の添え字
   * 入力:int x: 配列の添え字
   *
   * 戻り:　配列の添え字
   */
  protected int getIndex ( int y, int x ) throws IndexOutOfBoundsException {
    // 境界が拡張される必要があるかどうかチェックする
    if ( x < xLow ) {
      if (( xHigh - x ) >= xSize ) {
        IndexOutOfBoundsException.throwIt() ;
      }
      xLow = x ;
    } else if ( x > xHigh ) {
      if (( x - xLow ) >= xSize ) {
        IndexOutOfBoundsException.throwIt() ;
      }
      xHigh = x ;
    }

    if ( y < yLow ) {
      if (( yHigh - y ) >= ySize ) {
        IndexOutOfBoundsException.throwIt() ;
      }
      yLow = y ;
    } else if ( y > yHigh ) {
      if (( y - xLow ) >= ySize ) {
        IndexOutOfBoundsException.throwIt() ;
      }
      yHigh = y ;
    }

    // 実際の添え字を返す
    return super.getIndex (( y < 0 ) ? ( y + ySize ) : y
                          ,( x < 0 ) ? ( x + xSize ) : x ) ;
  }
}

package JBot;

import stamp.util.os.* ;

/**
 * 迷路のための地図サポート
 * 
 * 基本的なバックトラックパス作成を作る
 */

public class mazeMap extends charMap {
  protected int x, y ;

  static final public char unknown = 0 ;
  static final public char obstacle = 1 ;
  static final public char traveled = 2 ;
  static final public char path = 3 ;

  static final public char north = 0 ;
  static final public char east = 1 ;
  static final public char south = 2 ;
  static final public char west = 3 ;

  /**
   * 迷路地図を作る
   */
  public mazeMap ( int ySize, int xSize ) {
    // 地図を設定する
    super(ySize,xSize);

    // 地図をクリアする
    setAll (unknown) ;

    // 現在の位置を設定し地図に印を付ける
    x = 0 ;
    y = 0 ;
    markTraveled() ;
  }

  /**
   * カーソル位置を動かす
   *
   * 入力:int direction: 動かす方向
   */
  public void move ( int direction ) {
    // １マス動かす
    switch (direction) {
    case north: y++; break ;
    case east:  x++; break ;
    case south: y--; break ;
    case west:  x--; break ;
    }
  }

  /**
   * 移動したときに現在の位置(y,x)を印す
   */
  public void markTraveled () {
    set ( y, x, traveled ) ;
  }

  /**
   * 現在の位置(y,x)の前の領域に印す
   */
  public void markObstacle (int direction) {
    int x1, y1 ;

    switch (direction) {
    default:
    case north:
      x1 = x ;
      y1 = y + 1 ;
      break ;

    case east:
      x1 = x + 1 ;
      y1 = y ;
      break ;

    case south:
      x1 = x ;
      y1 = y - 1 ;
      break ;

    case west:
      x1 = x - 1 ;
      y1 = y ;
      break ;
    }

    set ( y1, x1, obstacle ) ;
  }

  /**
   * unknownと印されたマスに対して単純な最初の調査
   * バックトラックパスを作る間に地図を変える
   * 戻る前にオリジナルな状態に地図を戻す
   *
   * 入力;char[] backtrackPath: 場所のパスの配列
   *
   * 戻り:int: パスの要素 (0=no path)
   */
  public int computeBacktrackPath( char backtrackPath[]) {
    int x, y, step, next ;
    int pathSize = 0 ;
    int pathIndex = 1 ;

    pathIndex = 0 ;
    pathSize = 1 ;
    x = this.x ;
    y = this.y ;
    backtrackPath[0]=north;
    set(y,x,path);

    searchLoop: while(true) {
      switch (backtrackPath[pathIndex]) {
        default:
        case north: next = get(y+1,x) ; break;
        case east:  next = get(y,x+1) ; break;
        case south: next = get(y-1,x) ; break;
        case west:  next = get(y,x-1) ; break;
      }

      switch (next) {
      default:
      case path:
      case obstacle:
        // この方向には行けない。他を試す
        while ( backtrackPath[pathIndex] == west ) {
          // 探索が完了したかどうかをチェックする
          if ( pathSize == 1 ) {
            // 全てが終わった。全てが探索した。
            pathSize = 0 ;
            break searchLoop ;
          } else {
            // cut back path
            -- pathSize ;
            -- pathIndex ;

            // 現在の調査座標を戻す
            switch (backtrackPath[pathIndex]) {
              default:
              case north: --y ; break;
              case east:  --x ; break;
              case south: ++y ; break;
              case west:  ++x ; break;
            }
          }
        }

        // 次の方向を試す
        backtrackPath[pathIndex] ++ ;
        break;

      case traveled:
        // パス位置の終わりを変更する
        switch (backtrackPath[pathIndex]) {
        default:
        case north: ++y ; break;
        case east:  ++x ; break;
        case south: --y ; break;
        case west:  --x ; break;
        }

        // 循環パスを防ぐためにパスの部分としてマスに印す
        set(y,x,path);

        // 次の調査を設定する
        ++ pathSize ;
        ++ pathIndex ;
        backtrackPath[pathIndex]=north;
        break;

      case unknown:
        // パスを見つけた。地図のパスをリセットする
        x = this.x ;
        y = this.y ;

        for ( int i = 0 ; i < pathSize ; ++ i ) {
          // 移動したパスのマスを戻す
          set(y,x,traveled);

          // 次のマスが何処かを決定する
          switch (backtrackPath[i]) {
            default:
            case north: ++y; break;
            case east:  ++x; break;
            case south: --y; break;
            case west:  --x; break;
          }
        }

        break searchLoop ;
      }
    }

    // 全てが終わった。地図を戻す。
    for ( y = yLow ; y <= yHigh ; ++ y ) {
      for ( x = xLow ; x <= xHigh ; ++ x ) {
        if ( get(y,x) == path ) {
          set(y,x,traveled);
        }
      }
    }

    return pathSize ;
  }
}

package JBot ;

import stamp.core.*;
import stamp.util.os.* ;

/**
 * Printable mazeMap class
 * 
 * mazeMapとパスの詳細をプリントする
 */

public class printableMazeMap extends mazeMap{
  public char path[] ;

  /**
   * プリントできるmazeMapオブジェクトを作る
   */
  public printableMazeMap ( int y, int x ) {
    super( y, x ) ;
    path = new char [ 128 ] ;
  }

  /**
   * EEPROMに地図を保存する
   */
  public void save() {
    EEPROM.write(0,(byte)ySize);
    EEPROM.write(1,(byte)xSize);
    EEPROM.write(2,(byte)(xSize+ySize));
    EEPROM.write(3,(byte)yLow);
    EEPROM.write(4,(byte)yHigh);
    EEPROM.write(5,(byte)xLow);
    EEPROM.write(6,(byte)xHigh);

    int i = 7 ;
    for ( int y = yLow ; y <= yHigh ; ++ y ) {
      for ( int x = xLow ; x <= xHigh ; ++ x, ++i ) {
        EEPROM.write(i,(byte)get(y,x));
      }
    }
  }

  /**
   * EEPROMから地図をロードする
   */
  static public printableMazeMap load() {
    printableMazeMap map ;
    int x, y ;
    y = EEPROM.read(0);
    x = EEPROM.read(1);
    if ((x+y) == EEPROM.read(2)) {
      map = new printableMazeMap(y,x);
      map.realLoad();
    } else {
      map = new printableMazeMap(1,1);
    }
    return map;
  }

  /**
   * EEPROMから符号付byteを読む
   *
   * 入力:int i: EEPROMの添え字
   *
   * 戻り:int: 符号付byte
   */
  static protected int EEPROMread(int i) {
    int result = EEPROM.read(i);

    return ( result < 128 ) ? result : ( result - 256 ) ;
  }

  /**
   * EEPROMから地図をロードする
   */
  protected void realLoad() {
    resetBounds();
    yLow  = EEPROMread(3);
    yHigh = EEPROMread(4);
    xLow  = EEPROMread(5);
    xHigh = EEPROMread(6);

    int i = 7 ;
    for ( int y = yLow ; y <= yHigh ; ++ y ) {
      for ( int x = xLow ; x <= xHigh ; ++ x, ++i ) {
        set(y,x,EEPROM.read(i));
      }
    }
  }

  /**
   * 現在のバックトラックパスを探し、それをプリントする
   */
  public void printPath () {
    int pathSize;
    char c ;

    pathSize = computeBacktrackPath ( path ) ;

    if ( pathSize == 0 ) {
      System.out.println ( "No path found" ) ;
    } else {
      System.out.print ( "Path = " ) ;
      for ( int i = 0 ; i < pathSize ; ++ i ) {
        switch(path[i]){
        default:
        case north: c='N'; break;
        case east:  c='E'; break;
        case south: c='S'; break;
        case west:  c='W'; break;
        }
        System.out.print ( c ) ;
      }
      System.out.println ( " " ) ;
    }
  }


  protected void print ( String prefix, int y, int x ) {
    System.out.print ( prefix ) ;
    System.out.print ( " " ) ;
    System.out.print ( y ) ;
    System.out.print ( " " ) ;
    System.out.print ( x ) ;
    System.out.println ( " " ) ;
  }

  /**
   * 地図をプリントする
   *
   * 入力:Sting title: 地図の表題
   */
  public void print ( String title ) {
    char next, c ;

    System.out.println ( " " ) ;
    System.out.println ( title ) ;
    print ( "Y range", yLow(), yHigh()) ;
    print ( "X range", xLow(), xHigh()) ;

    for ( int y = yHigh ; y >= yLow ; -- y ) {
      if ( y >= 0 ) {
        System.out.print ( " " ) ; // handle leading sign
      }
      System.out.print ( y ) ;
      System.out.print ( ":" ) ;
      next    = ' ' ;

      for ( int x = xLow ; x <= xHigh ; ++ x ) {
        if (( this.y == y ) && ( this.x == x )) {
          // put [] around current position
          System.out.print ( '[' ) ;
          next = ']' ;
        } else {
          System.out.print ( next ) ;
          next = ' ' ;
        }

        switch (get(y,x)) {
        default:       c = '?' ; break;
        case unknown:  c = '.' ; break;
        case obstacle: c = 'O' ; break;
        case traveled: c = '+' ; break;
        }

        System.out.print (c) ;
      }
      System.out.println ( next ) ;
    }
  }
}

import stamp.core.*;
import stamp.util.os.* ;
import JBot.* ;

/**
 * MapMazeTaskクラスをテストする
 * 
 * J-Botは地図を作る間迷路を探索する
 */

public class MazeMapTest1 {
  public static void main () {
    printableMazeMap test = new printableMazeMap ( 20, 20 ) ;

    test.print("Initial");
    test.printPath();

    // 障害物を置いて地図を作る
/*
    for ( int i = 0 ; i < 2 ; ++ i ) {
      test.move(mazeMap.north);
      test.markTraveled();
      test.markObstacle(mazeMap.east);
      test.markObstacle(mazeMap.west);
    }

    test.move(mazeMap.north);
    test.markTraveled();
    test.markObstacle(mazeMap.north);
    test.markObstacle(mazeMap.west);
    test.move(mazeMap.east);
    test.markTraveled();
    test.markObstacle(mazeMap.north);
    test.markObstacle(mazeMap.east);
*/

    test.move(mazeMap.north);
    test.markTraveled();
    test.markObstacle(mazeMap.north);
    test.markObstacle(mazeMap.east);
    test.markObstacle(mazeMap.west);
    test.move(mazeMap.south);
    test.markObstacle(mazeMap.east);
    test.markObstacle(mazeMap.south);

    test.print("Basic movement");
    test.printPath();
  }
}