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

/*---解迷宫路径到path数组函数---*/
void build_path(int i, int j, int len, int *path, int *copy_wall)
{
    int k;
    int size_x = mark.size();
    int size_y = mark[0].size();

    for (k = len -1; k >= 0; k--)
    {
        if (i > 0  &&  ! copy_wall[i][j][1]  &&  map[i-1][j] == k)   //前一个单位的东面是当前格，即当前格子的左侧为上一个格子
        {
            i--;
            path[k] = 3;
        }
        else if (i < size_x - 1  &&  ! copy_wall[i+1][j][1]  &&  map[i+1][j] == k)   //前一个单位的西面是当前格，即当前格子的右侧为上一个格子
        {
            i++;
            path[k] = 1;
        }
        else if(j > 0  &&  ! copy_wall[i][j][0]  &&  map[i][j-1] == k)   //前一个单位的北面是当前格，即当前格子的下侧为上一个格子
        {
            j--;
            path[k] = 0;
        }
        else if (j < size_y - 1  &&  ! copy_wall[i][j+1][0]  &&  map[i][j+1] == k)   //前一个单位的南面是当前格，即当前格子的上侧为上一个格子
        {
            j++;
            path[k] = 2;
        }
        else
        {
            LCDPrintf("ERROR");
        }
    }
}

/*---通过path数组移动到目标点函数---*/
void drive_path(int len, int *path)
{
    for(i = 0; i < len; i++)
    {
        go_to(path[i]);
    }
}