
#ifndef GESTALT_PLANNER_API
#define GESTALT_PLANNER_API

#include <array>
#include <cmath>
#include <memory>
#include <string>
#include <valarray>
#include <vector>
#include <unordered_set>
#include <map>
#include <unordered_map>

using std::array;
using std::nan;
using std::shared_ptr;
using std::string;
using std::valarray;
using std::vector;

// we use NaN to represent an unchanged value.
const double unchangedNaN = nan("");

struct PoseUpdate;

//py::implicitly_convertible<PoseUpdate, Pose>();
struct Pose {
	double x = 0.0;
	double y = 0.0;
	double z = 0.0;
	double qx = 0.0;
	double qy = 0.0;
	double qz = 0.0;
	double qw = 1.0;
};

// convenience object for brace initialization,
// can be passed as a pose argument whose elements default to "unchanged".
struct PoseUpdate {
	double x = unchangedNaN;
	double y = unchangedNaN;
	double z = unchangedNaN;
	double qx = unchangedNaN;
	double qy = unchangedNaN;
	double qz = unchangedNaN;
	double qw = unchangedNaN;

	operator Pose() const { return Pose{ x, y, z, qx, qy, qz, qw }; }
};

struct Collision {
	int step = -1;
	std::string link1_id = "";
	std::string link2_id = "";
	std::valarray<double> position = { nan(""), nan(""), nan("") };
};

struct PlannerParamInfo {
	string defaultValue;
	string rangeSuggestion;
	// string help; // going to scrape that info from the ompl source
};

struct PlannerInfo {
	bool multithreaded;
	bool directed;
	std::map<string, PlannerParamInfo> params;
};

class PlannerState;

class GestaltPlanner {

private:
	// we hold the state like this so we can copy the planner instance around
	std::shared_ptr<PlannerState> state;

#include "plannermethods.inc" // autogenerated

// public: void json_rpc(string call);

private:
	friend class GestaltPlannerTest;
	// friend bool operator==(const GestaltPlanner&, const GestaltPlanner&);
};

std::string to_json(const Pose& pose);
std::string to_json(const Collision& collision);
std::string to_json(const GestaltPlanner& planner);

Pose from_json(const std::string& jsn);
// Collision from_json(const std::string& jsn);
// GestaltPlanner from_json(const std::string& jsn);

// bool operator==(const GestaltPlanner& lhs, const GestaltPlanner& rhs);

#endif