/**
 * space_server 任务路径管理。
 *
 * © 2020 成都河狸智能科技有限责任公司。保留所有权利。
 *
 * 作者: zhq1229
 */

#ifndef __PATH_H__
#define __PATH_H__

#include <map>
#include <vector>
#include <string>
#include <memory>
#include "task/map.h"

using PointId = unsigned int;
using PathId = unsigned int;

namespace task {

namespace path {

struct ChildPoint {
    ChildPoint():
        ptr(nullptr),
        g(0.0f) {}

    std::shared_ptr<struct PathPoint> ptr;
    float g;
};

struct AddPoint {
    AddPoint():
        forward(nullptr),
        reverse(nullptr),
        forward_ang(0.0f),
        reverse_ang(0.0f),
        max_vel(0.6f) {}

    std::shared_ptr<struct PathPoint> forward;
    std::shared_ptr<struct PathPoint> reverse;
    float forward_ang;
    float reverse_ang;
    float max_vel;
};

struct PathPoint {
    PathPoint():
        point_id(0),
        path_id(0),
        x(0.0f),
        y(0.0f),
        yaw(0.0f),
        type(""),
        father(nullptr),
        g(0.0f),
        h(0.0f),
        f(0.0f),
        closed(false) {
        children.clear();
    }

    PathPoint(
        PointId point_idt,
        PathId path_idt,
        float xt,
        float yt,
        float yawt,
        std::string typet = ""):
        point_id(point_idt),
        path_id(path_idt),
        x(xt),
        y(yt),
        yaw(yawt),
        type(typet),
        father(nullptr),
        g(0.0f),
        h(0.0f),
        f(0.0f),
        closed(false) {}

    void add_child(
        std::shared_ptr<struct PathPoint> ptr, float g) {
        struct ChildPoint c;
        c.ptr = ptr;
        c.g = g;
        children.push_back(c);
    }

    void set_plan(
        std::shared_ptr<struct PathPoint> fa, float _g, float _h) {
        father = fa;
        g = _g;
        h = _h;
        f = g + h;
    }

    void close() {
        closed = true;
    }

    void setAP(
        std::shared_ptr<struct PathPoint> f,
        std::shared_ptr<struct PathPoint> r,
        float forward_ang, float reverse_ang,
        float max_vel) {
        ap.forward = f;
        ap.reverse = r;
        ap.forward_ang = forward_ang;
        ap.reverse_ang = reverse_ang;
        ap.max_vel = max_vel;
    }

    PointId point_id;
    PathId path_id;
    float x;
    float y;
    float yaw;
    std::string type;
    std::vector<struct ChildPoint> children;
    AddPoint ap;
    std::shared_ptr<struct PathPoint> father;
    float g, h, f;
    bool closed;
};

struct PointInfo
{
    PointId point_id;
    std::string type;
    std::string name;
    float x;
    float y;
    float yaw;
};

struct PathInfo {
    PathInfo():
        path_id(0),
        name(""),
        type(""),
        dir("forbid"),
        max_vel(0.6f),
        forward_ang(0.0f),
        reverse_ang(0.0f) {}
    PathId path_id;
    MapId map_id;
    std::string name;
    std::string type;
    std::string dir;
    float max_vel;
    float forward_ang;
    float reverse_ang;
    std::vector<PointInfo> points;
};

using PathPointPtr = std::shared_ptr<struct PathPoint>;
using Path = std::vector<PathPointPtr>;
using Paths = std::vector<PathPointPtr>;
using MapPaths = std::map<MapId, Paths>;
using PathInfos = std::map<PathId, PathInfo>;
using MapPathInfos = std::map<MapId, PathInfos>;

Paths paths(MapId map_id);
bool paths(MapId map_id, Paths &paths);
PathInfos pathInfos(MapId map_id);
PathPointPtr pathPointPtr(Paths &paths, PointId point_id);
PathPointPtr onPathPointPtr(Paths &paths, float x, float y, float error);
int init();
int shutdown();

}  // namespace path

}  // namespace task

#endif  // __PATH_H__