#ifndef _JFX_MAP_TYPES_HPP_
#define _JFX_MAP_TYPES_HPP_

#include <cmath>
#include <map>
#include <set>
#include <string>
#include <vector>

namespace jf {
double GetValidZoneWidth(const struct Point &l, const struct Point &r);
struct XYZ P2XYZ(const struct Point &p);
struct Point XYZ2P(const struct XYZ &xy);
struct XYZ P2XYZ(const struct Point &p, double zone_width);
struct Point XYZ2P(const struct XYZ &xy, double zone_width);

/**
 * @brief 84坐标系中的点
 *
 */
struct Point {
    Point() : dLon(0), dLat(0), dAlt(0){};
    Point(double inlon, double inlat, double alt);
    Point(const Point &o);
    Point(const std::vector<double> &d);
    Point(const std::map<std::string, double> &m);

    /**
     * @brief 经度
     *
     */
    double dLon;
    /**
     * @brief 维度
     *
     */
    double dLat;
    /**
     * @brief 高度
     *
     */
    double dAlt;

    Point &operator=(const Point &o);
    bool operator==(const Point &o) const { return dLon == o.dLon && dLat == o.dLat && dAlt == o.dAlt; }
    bool operator!=(const Point &o) const { return !(*this == o); }
    std::string to_string() const { return std::to_string(dLon) + ", " + std::to_string(dLat) + ", " + std::to_string(dAlt); }
};

double PointsDis(const Point &l, const Point &r, bool consider_alt = false);
double PointsDis(double lng1, double lat1, double lng2, double lat2);
double Degree2Rad(double angle);
double PointsAngle(const Point &s, const Point &e);
double AngleSub(double dAngle1, double dAngle2);

// 已知一个点和距离该点横纵向的距离（可能为负），计算该点的坐标
void PointDis2NewPoint(const Point &ptInPoint, double dLonDis, double dLatDis, Point &ptOutPoint);

inline std::string ToString(const Point &p) { return std::to_string(p.dLon) + ", " + std::to_string(p.dLat) + ", " + std::to_string(p.dAlt); }

bool operator<(const Point &l, const Point &r);

/**
 * @brief 高斯坐标系中的点
 *
 */
struct XYZ {

    double x;
    double y;
    double z;
    XYZ operator-(const XYZ &other) const { return XYZ(x - other.x, y - other.y, z - other.z); }
    XYZ operator/(double size) const { return XYZ(x / size, y / size, z / size); }
    XYZ operator+(const XYZ &other) const { return XYZ(x + other.x, y + other.y, z + other.z); }
    XYZ operator*(double size) const { return XYZ(x * size, y * size, z * size); }
    double length() const { return sqrt(x * x + y * y + z * z); }
    double length_xy() { return sqrt(x * x + y * y); }
    XYZ() : x(0), y(0), z(0) {}
    XYZ(double inx, double iny, double inz) {
        x = inx;
        y = iny;
        z = inz;
    }
    XYZ normal() { return *this / length(); }
};

inline bool operator==(const XYZ &l, const XYZ &r) { return l.x == r.x && l.y == r.y; }

inline bool operator!=(const XYZ &l, const XYZ &r) { return !(l == r); }

inline XYZ norm(const XYZ &in) {
    double len = in.length();
    return XYZ(in.x / len, in.y / len, in.z / len);
}

inline XYZ norm(const XYZ &from, const XYZ &to) {
    XYZ delta = to - from;
    return delta / delta.length();
}

/**
 * @brief 84坐标系下TileId
 *
 */
struct TileId {
    TileId() : nLonId(0), nLatId(0) {}
    TileId(int inlonId, int inlatId);

    int nLonId;
    int nLatId;
    bool operator==(const TileId &o) { return nLonId == o.nLonId && nLatId == o.nLatId; }
    TileId getl() const { return TileId(nLonId - 1, nLatId); }
    TileId getr() const { return TileId(nLonId + 1, nLatId); }
    TileId gett() const { return TileId(nLonId, nLatId + 1); }
    TileId getb() const { return TileId(nLonId, nLatId - 1); }
    TileId getlb() const { return getl().getb(); }
    TileId getlt() const { return getl().gett(); }
    TileId getrb() const { return getr().getb(); }
    TileId getrt() const { return getr().gett(); }
    std::string to_string() const { return std::to_string(nLonId) + ", " + std::to_string(nLatId); }
};

bool operator<(const TileId &l, const TileId &r);

/**
 * @brief 84坐标系下区域
 *
 */
struct Rect2D {
    /**
     * @brief 左下角
     *
     */
    Point lb = {0, 0, 0};
    /**
     * @brief 右上角
     *
     */
    Point rt = {0, 0, 0};
    bool Contains(const Point &p) const;
    Rect2D() {}
    Rect2D getLt() const;
    Rect2D getLb() const;
    Rect2D getRt() const;
    Rect2D getRb() const;
    Rect2D(const Point &inlb, const Point &inrt);
    Rect2D(const Rect2D &o);
    Rect2D(const Point &p, float lon_interval_half, float lat_interval_half);
    bool intersect(const Rect2D &r);
};

inline Rect2D P2R(const Point &p, float lon_interval_half_metre, float lat_interval_half_metre) {
    XYZ xyzp = P2XYZ(p);
    XYZ xylb = XYZ(xyzp.x - lon_interval_half_metre, xyzp.y - lat_interval_half_metre, 0);
    XYZ xyrt = XYZ(xyzp.x + lon_interval_half_metre, xyzp.y + lat_interval_half_metre, 0);
    Point plb = XYZ2P(xylb);
    Point prt = XYZ2P(xyrt);
    return Rect2D(plb, prt);
}

inline TileId P2TId(const Point &p) {
    auto func_doubleToInt = [](double in) {
        int i = int(in);
        if (in - i > 0.999999) {
            return int(in + 1);
        }
        return i;
    };

    int lonId = func_doubleToInt(p.dLon / 0.02 /* MapConfig::tile_lon_interval */);
    int latId = func_doubleToInt(p.dLat / 0.02 /* MapConfig::tile_lat_interval */);
    return TileId(lonId, latId);
}

typedef int64_t JfDataId;

struct HdLinkId {
    HdLinkId() : lValue(0) {}
    HdLinkId(JfDataId in) { lValue = in; }
    HdLinkId(JfDataId inid, std::vector<TileId> intids) {
        lValue = inid;
        vcttiTileIds = intids;
    }

    JfDataId lValue = 0;
    std::vector<TileId> vcttiTileIds;
    const JfDataId operator()() const { return lValue; }
    operator JfDataId() const { return lValue; }
    HdLinkId mergeTileId(const std::vector<TileId> &intids) {
        vcttiTileIds = intids;
        return *this;
    }
    JfDataId GetLinkId() const { return lValue; }
    std::vector<TileId> GetTileIds() const { return vcttiTileIds; }
//    bool operator<(const HdLinkId &p) const
//    {
//        return lValue <  p.lValue;
//    }
};
inline std::string to_string(const HdLinkId &in) {
    std::string ret = std::to_string(in.lValue);
    return ret;
}

inline bool operator<(const HdLinkId &l, const HdLinkId &r) { return l.lValue < r.lValue; }
inline bool operator==(const HdLinkId &l, const HdLinkId &r) { return l.lValue == r.lValue; }
inline bool operator!=(const HdLinkId &l, const HdLinkId &r) { return l.lValue != r.lValue; }

struct HdLaneId {
    HdLaneId(JfDataId in) { lValue = in; }
    HdLaneId() : lValue(0) {}

    JfDataId lValue = 0;
    operator JfDataId() const { return lValue; }
//    bool operator<(const HdLaneId &p) const
//    {
//        return lValue <  p.lValue;
//    }
};
inline std::string ToString(const HdLaneId &in) {
    std::string ret = std::to_string(in.lValue);
    return ret;
}
// bool operator==(const HdLaneId &l, const HdLaneId &r) { return l.lValue == r.lValue; }

typedef JfDataId HdEdgeId;

struct LinkEndpoint {
    LinkEndpoint() : dLength(0.0) {}

    Point ptEpNode;                   // 坐标点（取车道的终点）
    HdLinkId lLinkId;                 // 所在Link的ID
    HdLaneId lLaneId;                 // 所在车道的ID：注意对于通过全联通车道线到达的点，ID取建立全联通后终点所在的车道线的ID
    double dLength;                   // 长度
    std::vector<Point> vctptGeoLink;  // 轨迹点
};

struct LaneEndpoint : LinkEndpoint {
    LaneEndpoint() : nStartLaneNum(0), nEndLaneNum(0) {}

    int nStartLaneNum;    // 同一组link内，到达当前点的起始点的车道号
    int nMidLaneNum = 0;  // 同一组link内，可能会有两次变道，如果该值不为0，则它表示第一次变道的终点车道同时也表示第二次变道的起始车道，如果为0则表示没有第二次变道
    int nEndLaneNum;      // 当前点所在的车道的车道号：注意对于通过全联通车道线到达的点，车道号取建立全联通后终点所在的车道线的车道号
};

typedef uint64_t RoadId;
struct SdNodeId {
    Point p;
    SdNodeId() : p(0, 0, 0) {}
    SdNodeId(const Point &inp) : p(inp) {}

    std::string to_string() const { return p.to_string(); }
    Point GetLinkId() const { return p; }
    TileId GetTileId() const { return P2TId(p); }
    std::vector<TileId> GetTileIds() const { return {P2TId(p)}; }
};

inline std::string ToString(const SdNodeId &id) { return jf::ToString(id.p); }

inline bool operator<(const SdNodeId &l, const SdNodeId &r) {
    if (l.p.dLon < r.p.dLon) {
        return true;
    } else if (l.p.dLon == r.p.dLon && l.p.dLat < r.p.dLat) {
        return true;
    }
    return false;
}
inline bool operator==(const SdNodeId &l, const SdNodeId &r) { return l.p.dLon == r.p.dLon && l.p.dLat == r.p.dLat; }
inline bool operator!=(const SdNodeId &l, const SdNodeId &r) { return !(l == r); }

struct BindRoadInput {
    Point ingps;
    std::set<RoadId> prior_protect_rids;
    std::set<RoadId> prior_punish_rids;
    std::set<SdNodeId> prior_protect_node;  // 在这个集合中的点概率会有加成
    std::set<SdNodeId> prior_punish_node;   // 在这个集合中的点概率会有消减
};

struct BindRoadOutput {
    BindRoadOutput(size_t inCandidateSize = 1) : e(BindRoadOutput::error::no_error), candidateSize(inCandidateSize) {}
    enum error {
        no_error,               // 无错
        no_candidates_in_init,  // init时附近没有点
        failed_after_punish,    // punish后无点
        no_candidates,          // 附近没有点，算法重启
        pending_after_init      // 算法启动后，第1次绑路点通常是这个状态，第2次绑路点如果是第1次绑路点可达，状态才会变成no_error
    };
    error e = BindRoadOutput::no_error;

    Point inp = {0, 0, 0};
    struct nodeInfo {
        nodeInfo(RoadId _rid, Point _outp, float _obsProb, float _transProb, float _accProb) : rid(_rid), outp(_outp), obsProb(_obsProb), transProb(_transProb), accProb(_accProb){};
        RoadId rid = 0;
        Point outp = {0, 0, 0};
        float obsProb = 0;    // 观察概率
        float transProb = 0;  // 转移概率
        float accProb = 0;    // 累计概率
    };
    std::vector<nodeInfo> nodeInfos;
    size_t candidateSize;

    // void test_print_qgis() {
    //     const std::string fileName = "bindroad_qgis";
    //     for (nodeInfo &ni : nodeInfos) {
    //         FileSys::AppendSlow(fileName, jf::to_string(inp) + "," + jf::to_string(ni.outp) + "," + std::to_string(ni.obsProb) + "," + std::to_string(ni.transProb) + "," + std::to_string(ni.accProb) + "\n");
    //     }
    // }
};

////////////////////////////////////////////////////////////////
}  // namespace jf

#endif  // _JFX_MAP_TYPES_HPP_