#include <vector>
#include "ddAStar.h"


// std::vector<std::vector<int>> _grid;
std::vector<std::vector<int>> grid;
int resolution = 2;
int rr = 4;
AStar aStar(grid, resolution, rr);
//
PointInt start(10, 10);
PointInt goal(30, 30);  // 目标


void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);
  Serial.println("Hello, ESP32!");


  std::vector<Point> pt_list;
  // 实际坐标
  for (int i = -10; i < 60; ++i) {
    pt_list.push_back(Point(i, -10));  // 右边线
  }
  for (int i = -10; i < 60; ++i) {
    pt_list.push_back(Point(60, i));  // 上边线
  }
  for (int i = -10; i < 61; ++i) {
    pt_list.push_back(Point(i, 60));  // 下边线
  }
  for (int i = -10; i < 61; ++i) {
    pt_list.push_back(Point(-10, i));  // 左边线
  }
  //
  for (int i = -10; i < 40; ++i) {
    pt_list.push_back(Point(20, i));  // 左短线1
  }
  for (int i = 0; i < 40; ++i) {
    pt_list.push_back(Point(40, 60 - i));  // 右短线
  }

  Serial.println(pt_list.size());


  grid = aStar.obstacle_list_to_grid1(pt_list);  // 将实际障碍坐标转为网格数据， 障碍物地图生成：根据障碍物坐标(ox,oy)确定地图边界等...


  Serial.println(grid.size());
  Serial.println("");
  for (size_t i = 0; i < grid.size(); ++i) {
    for (size_t j = 0; j < grid[i].size(); ++j) {
      Serial.print(grid[i][j]);
    }
    Serial.println("");
  }
  Serial.println("");

  Serial.print(start.x);
  Serial.print(", ");
  Serial.println(start.y);

  long startTime = millis();
  std::vector<PointInt> path = aStar.astarFindPath(start, goal);  // 寻路
  Serial.print("耗时：");
  Serial.println(millis() - startTime);

  // 打印路径
  for (size_t i = 0; i < path.size(); ++i) {
    Serial.print("(");
    Serial.print(path[i].x);
    Serial.print(",");
    Serial.print(path[i].y);
    Serial.print("),");
  }

  Serial.println("");
  startTime = millis();
  std::vector<PointInt> path_ok = aStar.path_optimal1(path); // 优化路径
  Serial.print("耗时2：");
  Serial.println(millis() - startTime);

  // 打印路径
  for (const auto& p : path_ok) {
    Serial.print("(");
    Serial.print(p.x);
    Serial.print(",");
    Serial.print(p.y);
    Serial.print(")");
  }
  Serial.println("");

  float ddd = 49.0 / 2;
  int ddd2 = round(ddd);
  Serial.println(ddd);
  Serial.println(ddd2);

}

void loop() {
  // put your main code here, to run repeatedly:
  delay(1000); // this speeds up the simulation



  long startTime = millis();
  std::vector<PointInt> path = aStar.astarFindPath(start, goal);  // 寻路
  Serial.print("耗时：");
  Serial.println(millis() - startTime);

  // // 打印路径
  // for (size_t i = 0; i < path.size(); ++i) {
  //   Serial.print("(");
  //   Serial.print(path[i].x);
  //   Serial.print(",");
  //   Serial.print(path[i].y);
  //   Serial.print("),");
  // }
  // Serial.println("");

  startTime = millis();
  std::vector<PointInt> path_ok = aStar.path_optimal1(path); // 优化路径
  Serial.print("耗时2：");
  Serial.println(millis() - startTime);

  // 打印路径
  for (const auto& p : path_ok) {
    Serial.print("(");
    Serial.print(p.x);
    Serial.print(",");
    Serial.print(p.y);
    Serial.print(")");
  }
  Serial.println("");

}