#include <planner.h>
namespace planner = pln

// difinition of two-dimensional space
const int DIM = 2;
pln::EuclideanSpace space(DIM);

// definition of bounds of each dimension
std::vector<pln::Bound> bounds{pln::Bound(0, 100.0),
                               pln::Bound(0, 100.0)};

// set bounds to space
space.setBound(bounds);

// definition of obstacle (point cloud type)
std::vector<pln::PointCloudConstraint::Hypersphere> obstacles;
obstacles.emplace_back(pln::State(10.0, 20.0),  10.0);  // x : 10.0, y : 20.0, radius : 10.0
obstacles.emplace_back(pln::State(50.0, 70.0),  20.0);  // x : 50.0, y : 70.0, radius : 20.0
obstacles.emplace_back(pln::State(-10.0, 120.0), 30.0); // there is no probrem out of range

// definition of constraint using std::shared_ptr
auto constraint = std::make_shared<pln::PointCloudConstraint>(space, obstacles)

// read image
auto world = cv::imread("./example.png", CV_8UC1);

// definition of constraint array
std::vector<pln::ConstraintType> map(world.cols * world.rows, pln::ConstraintType::ENTAERABLE);

for(int yi = 0; yi < world.rows; yi++) {
    for(int xi = 0; xi < world.cols; xi++) {
        if(world.data[xi + yi * world.cols] != 255) {
            map[xi + yi * world.cols] = pln::ConstraintType::NOENTRY;
        }
    }
}

std::vector<uint32_t> each_dim_size{(uint32_t)world.cols, (uint32_t)world.rows};

// definition of constraint using std::shared_ptr
auto constraint = std::make_shared<pln::GridConstraint>(space, map, each_dim_size);

// definition of planner (you can set some parameters at optional argument)
// pln::RRT planner(DIM);
// pln::RRTStar planner(DIM);
pln::InformedRRTStar planner(DIM);

// set constraint
planner.setProblemDefinition(constraint);

// definition of start and goal state
pln::State start(5.0, 5.0);
pln::State goal(90.0, 90.0);

// solve
bool status = planner.solve(start, goal);
if(status) {
    auto& result = planner.getResult();
    for(const auto& r : result) {
        std::cout << r << std::endl;
    }
}
else {
    std::cout << "Could not find path" << std::endl;
}