#include<iostream>
#include<string>
#include"eyebot++.h"
#include"maze_parameter.h"
#include"maze_func.h"
using namespace std;

/*---X坐标更新---*/
void xneighbor(int x, int dir)
{
  int maze_dir = (dir + 4) % 4;
  switch (maze_dir)
  {
    case 0:
    break;
    
    case 1:
    rob_x = x-1;
    break;
    
    case 2:
    break;
    
    case 3:
    rob_x = x+1;
    break;
  }
}

/*---Y坐标更新---*/
void yneighbor(int y, int dir)
{
  int maze_dir = (dir + 4) % 4;
  switch (maze_dir)
  {
    case 0:
    rob_y = y+1;
    break;
    
    case 1:
    break;
    
    case 2:
    rob_y = y-1;
    break;
    
    case 3:
    break;
  }
}

/*---行驶到指定单元格---*/
void go_to(int dir)
{
  int turn;   //定义转向倍率
  int maze_dir;
  //static int cur_x,cur_y,cur_p;
  
  maze_dir = (dir + 4) % 4;  //保证机器人dir方位在[0-3]之间
  turn = maze_dir - rob_dir;   //旋转角度倍数
  //turn = turn == +3 ? turn = -1 : turn = turn;  三目运算符？能用吗？
  
  if (turn == +3) //确保机器人是旋转90度而不是270度，减少时间
  {
    turn = -1;
  }
  if (turn == -3)
  {
    turn = +1;
  }
  
  if (turn) //如果turn不是0，则执行旋转命令
  {
    BOTturn(turn);
    VWWait();
  }
  PIDStraight();   //使用PID算法算法，向指定方向直线行驶一格
  VWWait();   //等待下一条指令
  
  rob_dir = maze_dir;  //更新机器人移动到下一格之后的方向
  xneighbor(rob_x,rob_dir); //更新机器人移动到下一格之后的X坐标
  yneighbor(rob_y,rob_dir); //更新机器人移动到下一格之后的y坐标
}