#include "path_plan.h"

#include "stdio.h"
#include "stdlib.h"

// 全局路径规划器
PathPlanner pathPlanner;

void SetPath();

// 初始化路径规划器
void PathPlanner_Init()
{
    pathPlanner.currentStep = -1;
    pathPlanner.path = NULL;
    pathPlanner.pathLength = 0;
    SetPath();
}

// 设置自定义路径
void SetPath()
{
    int i;
    for (i = 0; i < 100; i++)
    {
        AddPathStep(LEFT);
        AddPathStep(RIGHT);
        AddPathStep(STRAIGHT);
    }
}

// 获取当前方向
Direction GetCurrentDirection()
{
    return pathPlanner.path[pathPlanner.currentStep];
}

// 获取下一步方向
Direction GetNextDirection()
{
    if (pathPlanner.currentStep >= pathPlanner.pathLength)
    {
        pathPlanner.currentStep = -1;
    }
    return pathPlanner.path[++pathPlanner.currentStep];
}

// 添加路径
void AddPathStep(Direction step)
{
    pathPlanner.path = realloc(pathPlanner.path, ++pathPlanner.pathLength * sizeof(Direction));
    pathPlanner.path[pathPlanner.pathLength - 1] = step;
}