#ifndef sdimporter_h
#define sdimporter_h

#include <memory>

#include "MapConfig.h"
#include "SdTypes.h"
#include "Tile.h"
#include "Utils.h"
#include "World.h"

namespace jf {
class SdImporter {
   private:
    struct ImportNodeCallback {
        SdImporter *i;
        void operator()(const SdNode &n) {
            std::shared_ptr<SdTile> t = PutNodeToWorld(n);
            i->m.insert(std::make_pair(n.id_p, t));
        }

        std::shared_ptr<SdTile> PutNodeToWorld(const SdNode &nd) {
            TileId tid = P2TId(nd.id_p.p);
            std::shared_ptr<SdTile> t = SdWorld::GetSingleton()->FindOrAddTile(tid);
            const_cast<SdNode &>(nd).interpolated_node = false;
            t->PutNode(nd.id_p.p, nd.id_p, nd);
            return t;
        }

        ImportNodeCallback(SdImporter *ini) : i(ini) {}
    };

    struct ImportWayCallback {
        SdImporter *im;
        void operator()(const BaseWay &way) {
            if (way.nodes.empty()) {
                return;
            }

            auto Import = [this](const BaseWay &way) {
                // 将整个link的方向作为上边所有点的参考方向
                // 1. link几乎都比较直
                // 2. 方向在绑路中只是用来参考
                // 地图中有环形路段
                // assert(start != end);
                std::shared_ptr<SdTile> t_start = nullptr;
                std::shared_ptr<SdTile> t_end = nullptr;
                assert(Utils::MapValue(im->m, way.nodes[0], t_start));
                assert(Utils::MapValue(im->m, way.nodes[way.nodes.size() - 1], t_end));
                const SdNode *startNodeData = t_start->GetNodeData(way.nodes[0]);
                const SdNode *endNodeData = t_end->GetNodeData(way.nodes[way.nodes.size() - 1]);
                assert(startNodeData);
                assert(endNodeData);
                Rot r(jf::P2XYZ(startNodeData->id_p.p), jf::P2XYZ(endNodeData->id_p.p));

                for (int i = 0; i < int(way.nodes.size()) - 1; i++) {
                    SdNodeId from = way.nodes[i];
                    SdNodeId to = way.nodes[i + 1];
                    std::shared_ptr<SdTile> t_from = nullptr;
                    assert(Utils::MapValue(im->m, from, t_from));
                    PutLinkToWorld(im, from, t_from->GetTId(), to, r, way);
                }
            };
            BaseWay wayback = way;
            std::reverse(wayback.nodes.begin(), wayback.nodes.end());
            if (way.direction == 1) {
                Import(way);
                Import(wayback);
            } else if (way.direction == 2) {
                Import(way);
            } else if (way.direction == 3) {
                Import(wayback);
            }
        }

        void PutLinkToWorld(class SdImporter *i, SdNodeId fromnid, TileId fromtid, SdNodeId tonid, Rot &r, const BaseWay &wayData) {
            const SdNode *nd_from = SdWorld::GetSingleton()->GetNodeData(fromnid);
            const SdNode *nd_to = SdWorld::GetSingleton()->GetNodeData(tonid);
            assert(nd_from);
            assert(nd_to);
            // 包括起点终点
            std::vector<Point> interps;
            std::vector<SdRadas> rs;
            GetInterpolatePoints(nd_from, nd_to, interps, rs);

            // 至少包括起点终点
            assert(interps.size() >= 2);
            for (int i = 0, j = 1; i < interps.size() - 1; ++i, ++j) {
                Point p_from = interps[i];
                Point p_to = interps[j];
                SdNodeId nid_from = SdNodeId{p_from};
                SdNodeId nid_to = SdNodeId{p_to};

                TileId tid_from = P2TId(p_from);
                TileId tid_to = P2TId(p_to);

                std::shared_ptr<SdTile> tile_from = nullptr;
                std::shared_ptr<SdTile> tile_to = nullptr;
                // 这里可能没有,如下,右上tile没有数据,所以插值时候,可能插到右上
                // ___
                // |_|_
                // |_|_|
                //
                if (!SdWorld::GetSingleton()->GetTile(tid_from, tile_from)) {
                    tile_from = SdWorld::GetSingleton()->NewTile(tid_from, TId2Rect(tid_from));
                }
                if (!SdWorld::GetSingleton()->GetTile(tid_to, tile_to)) {
                    tile_to = SdWorld::GetSingleton()->NewTile(tid_to, TId2Rect(tid_to));
                }

                // 不是起点,因为起点已经PutNode了
                const SdNode *sdnd_from = tile_from->GetNodeData(nid_from);
                const SdNode *sdnd_to = tile_to->GetNodeData(nid_to);
                if (!sdnd_from) {
                    SdNode n_from{};
                    n_from.id_p = nid_from;
                    n_from.speed_limit_big = nd_from->speed_limit_big;
                    n_from.speed_limit_small = nd_from->speed_limit_small;
                    n_from.level = nd_from->level;
                    n_from.radas = rs[i];
                    n_from.interpolated_node = (nid_from != fromnid && nid_to != tonid);
                    tile_from->PutNode(n_from.id_p.p, n_from.id_p, n_from);
                }
                if (!sdnd_to) {
                    SdNode n_to{};
                    n_to.id_p = nid_to;
                    n_to.speed_limit_big = nd_from->speed_limit_big;
                    n_to.speed_limit_small = nd_from->speed_limit_small;
                    n_to.level = nd_from->level;
                    n_to.radas = rs[j];
                    n_to.interpolated_node = (nid_to != fromnid && nid_to != tonid);
                    tile_to->PutNode(n_to.id_p.p, n_to.id_p, n_to);
                }

                PutLinkToTile(nid_from, tile_from.get(), nid_to, tile_to.get(), wayData.id, wayData.level, r);
            }
        }

        void PutLinkToTile(SdNodeId nid_from, SdTile *t_from, SdNodeId nid_to, SdTile *t_to, RoadId wid, int level, Rot &r) {
            auto func_from = [nid_from, wid, nid_to, level, r, t_to](std::map<SdNodeId, SdNode> &_m) {
                SdNode _;
                assert(Utils::MapValue(_m, nid_from, _));
                SdNode &nd_from = _m[nid_from];
                nd_from.outDegree.push_back(nid_to);
                const SdNode *toNode = t_to->GetNodeData(nid_to);
                nd_from.outLengths.push_back(PointsDis(_m[nid_from].id_p.p, toNode->id_p.p));
                // assert(!util::VectorContains(nd_from.outRids, wid));
                nd_from.outRids.push_back(wid);
                nd_from.level = level;
            };
            t_from->UpdateNodesData(func_from);
            auto func_to = [nid_from, wid, nid_to, level, r, t_from](std::map<SdNodeId, SdNode> &_m) {
                SdNode _;
                assert(Utils::MapValue(_m, nid_to, _));
                SdNode &nd_to = _m[nid_to];
                nd_to.inDegree.push_back(nid_from);
                const SdNode *fromNode = t_from->GetNodeData(nid_from);
                nd_to.inLengths.push_back(PointsDis(_m[nid_to].id_p.p, fromNode->id_p.p));
                // assert(!util::VectorContains(nd_to.inRids, wid));
                nd_to.inRids.push_back(wid);
                nd_to.level = level;
            };
            t_to->UpdateNodesData(func_to);
        }

        void GetInterpolatePoints(const SdNode *from, const SdNode *to, std::vector<Point> &ret, std::vector<SdRadas> &rs) {
            if (from->id_p == to->id_p) {
                ret.push_back(from->id_p.p);
                ret.push_back(to->id_p.p);
            } else {
                double d = GetValidZoneWidth(from->id_p.p, to->id_p.p);
                XYZ xyz_from = P2XYZ(from->id_p.p, d);
                XYZ xyz_to = P2XYZ(to->id_p.p, d);
                double l = DXYZ(xyz_to, xyz_from);
                XYZ n = norm(xyz_from, xyz_to);
                // 坐标转换会造成误差,所以对已经设置的点不做二次转换
                ret.push_back(from->id_p.p);

                SdRadas radas_from = from->radas;
                SdRadas radas_to = to->radas;
#define radas_check(r) assert(r.curvature<1e100 && r.curvature> - 1e100 && r.roll < 1e100 && r.roll > -1e100 && r.pitch < 1e100 && r.pitch > -1e100 && r.azimuth < 1e100 && r.azimuth > -1e100)
                radas_check(radas_from);
                rs.push_back(radas_from);
                for (int i = 1; MapConfig::node_interpolate * i < l; ++i) {
                    XYZ interpo = xyz_from + n * (i * MapConfig::node_interpolate);
                    ret.push_back(XYZ2P(interpo, d));

                    SdRadas r;
                    double div = (double(MapConfig::node_interpolate * i) / l);
#define radas_interp(from, to, div) (from + (to - from) * div)
                    r.curvature = radas_interp(radas_from.curvature, radas_to.curvature, div);
                    r.roll = radas_interp(radas_from.roll, radas_to.roll, div);
                    r.pitch = radas_interp(radas_from.pitch, radas_to.pitch, div);
                    r.azimuth = radas_interp(radas_from.azimuth, radas_to.azimuth, div);
                    radas_check(r);
                    rs.push_back(r);
                }
                ret.push_back(to->id_p.p);
                radas_check(radas_to);
                rs.push_back(radas_to);
            }
        }
        ImportWayCallback(SdImporter *ini) : im(ini) {}
    };
    struct ImportBoundCallback {
        SdImporter *i;
        void operator()(const BaseBound &b) {}
        ImportBoundCallback(SdImporter *ini) : i(ini) {}
    };
    struct ImportRelationCallback {
        SdImporter *i;
        void operator()() {}
        ImportRelationCallback(SdImporter *ini) : i(ini) {}
    };
    ImportNodeCallback nc;
    ImportWayCallback wc;
    ImportBoundCallback bc;
    ImportRelationCallback rc;

   public:
    SdImporter() : nc(this), wc(this), bc(this), rc(this) {}
    ImportNodeCallback GetNodeCallback() { return nc; }
    ImportWayCallback GetWayCallback() { return wc; }
    ImportBoundCallback GetBoundCallback() { return bc; };
    ImportRelationCallback GetRelationCallback() { return rc; };
    std::shared_ptr<SdTile> GetTile(SdNodeId nid) const {
        std::shared_ptr<SdTile> ret = nullptr;
        assert(Utils::MapValue(m, nid, ret));
        return ret;
    }
    SdWorld *w;
    std::map<SdNodeId, std::shared_ptr<SdTile>> m;
};
}  // namespace jf

#endif