#include <maps/wikimap/mapspro/services/autocart/libs/post_processing/include/align_along_road.h>

#include <maps/libs/geolib/include/polygon.h>
#include <maps/libs/geolib/include/polyline.h>
#include <maps/libs/geolib/include/point.h>

#include <maps/libs/cmdline/include/cmdline.h>
#include <maps/libs/common/include/exception.h>
#include <maps/libs/log8/include/log8.h>

#include <opencv2/opencv.hpp>

#include <algorithm>
#include <fstream>
#include <vector>
#include <string>

namespace autocart = maps::wiki::autocart;
namespace geolib3 = maps::geolib3;

namespace {

std::vector<std::vector<geolib3::Point2>>
loadPts(const std::string& filename) {
    std::ifstream ifs(filename);
    REQUIRE(ifs.is_open(), "Failed to open file: " << filename);

    std::vector<std::vector<geolib3::Point2>> pts;
    while (!ifs.eof()) {
        std::string line;
        std::getline(ifs, line);
        if (line.empty()) {
            continue;
        }
        std::stringstream ss(line);
        int ptsCnt;
        ss >> ptsCnt;
        std::vector<geolib3::Point2> temp;
        temp.reserve(ptsCnt);
        for (int i = 0; i < ptsCnt; i++) {
            float x, y;
            ss >> x >> y;
            temp.emplace_back(x, y);
        }
        pts.push_back(temp);
    }

    return pts;
}

std::vector<geolib3::Polyline2>
convertToRoads(const std::vector<std::vector<geolib3::Point2>>& pts) {
    std::vector<geolib3::Polyline2> roads;
    roads.reserve(pts.size());
    for (const auto& road : pts) {
        roads.emplace_back(road);
    }
    return roads;
}

std::vector<geolib3::Polygon2>
convertToBlds(const std::vector<std::vector<geolib3::Point2>>& pts) {
    std::vector<geolib3::Polygon2> blds;
    blds.reserve(pts.size());
    for (const auto& bld : pts) {
        blds.emplace_back(bld);
    }
    return blds;
}

cv::Size getRequireCanvasSize(const std::vector<geolib3::Polygon2>& blds,
                              const std::vector<geolib3::Polyline2>& roads) {
    double maxX = 0;
    double maxY = 0;
    for (const auto& bld : blds) {
        size_t ptsCnt = bld.pointsNumber();
        for (size_t i = 0; i < ptsCnt; i++) {
            geolib3::Point2 pt = bld.pointAt(i);
            maxX = std::max(maxX, pt.x());
            maxY = std::max(maxY, pt.y());
        }
    }

    for (const auto& road : roads) {
        size_t ptsCnt = road.pointsNumber();
        for (size_t i = 0; i < ptsCnt; i++) {
            geolib3::Point2 pt = road.pointAt(i);
            maxX = std::max(maxX, pt.x());
            maxY = std::max(maxY, pt.y());
        }
    }

    return cv::Size(maxX, maxY);
}

void drawPolygons(cv::Mat* canvas,
                  const std::vector<geolib3::Polygon2>& polygons) {
    REQUIRE(canvas->channels() == 3, "Canvas should be BGR image");
    for (const auto& polygon : polygons) {
        size_t ptsCnt = polygon.pointsNumber();
        for (size_t i = 0; i  < ptsCnt; i++) {
            geolib3::Point2 pt0 = polygon.pointAt(i);
            geolib3::Point2 pt1 = polygon.pointAt((i + 1) % ptsCnt);

            cv::Point2d cvPt0(pt0.x(), pt0.y());
            cv::Point2d cvPt1(pt1.x(), pt1.y());

            cv::line(*canvas, cvPt0, cvPt1, cv::Scalar(0, 255, 0), 2);
        }
    }
}

void drawPolylines(cv::Mat* canvas,
                   const std::vector<geolib3::Polyline2>& lines) {
    REQUIRE(canvas->channels() == 3, "Canvas should be BGR image");
    for (const auto& line : lines) {
        size_t ptsCnt = line.pointsNumber();
        for (size_t i = 0; i < ptsCnt - 1; i++) {
            geolib3::Point2 pt0 = line.pointAt(i);
            geolib3::Point2 pt1 = line.pointAt(i + 1);

            cv::Point2d cvPt0(pt0.x(), pt0.y());
            cv::Point2d cvPt1(pt1.x(), pt1.y());

            cv::line(*canvas, cvPt0, cvPt1, cv::Scalar(0, 0, 255), 3);
        }
    }
}

cv::Mat drawObjects(const std::string& imgPath,
                    const std::vector<geolib3::Polygon2>& blds,
                    const std::vector<geolib3::Polyline2>& roads) {
    cv::Mat canvas;
    if (!imgPath.empty()) {
        canvas = cv::imread(imgPath);
        REQUIRE(!canvas.empty(), "Failed to load image:" << imgPath);
    } else {
        cv::Size canvasSize = getRequireCanvasSize(blds, roads);
        canvas = cv::Mat(canvasSize, CV_8UC3, cv::Scalar::all(0));
    }
    drawPolygons(&canvas, blds);
    drawPolylines(&canvas, roads);

    return canvas;
}

void dumpBlds(const std::string& filename,
              const std::vector<geolib3::Polygon2>& blds) {
    std::ofstream ofs(filename);
    for (const auto& bld : blds) {
        ofs << bld.pointsNumber();
        for (size_t i = 0; i < bld.pointsNumber(); i++) {
            geolib3::Point2 pt = bld.pointAt(i);
            ofs << " " << pt.x() << " " << pt.y();
        }
        ofs << std::endl;
    }
}

} // namespace

int main(int argc, char** argv)
try {
    maps::cmdline::Parser parser;
    auto imgPathParam = parser.string("img_path")\
            .defaultValue("")\
            .help("path to the satellite image file");

    auto bldPathParam = parser.string("bld_path")\
            .required()\
            .help("path to txt file with building polygons");

    auto roadPathParam = parser.string("road_path")\
            .required()\
            .help("path to txt file with road polylines");

    auto dumpResultBldParam = parser.string("dump_result_bld")\
            .defaultValue("")\
            .help("path to output file with building polygons");

    auto dumpInitialImageParam = parser.string("dump_initial_image")\
            .defaultValue("")\
            .help("dump image with initial polygons");

    auto dumpResultImageParam = parser.string("dump_result_image")\
            .defaultValue("")\
            .help("dump image with result polygons");

    parser.parse(argc, argv);

    INFO() << "Loading building polygons: " << bldPathParam;
    std::vector<std::vector<geolib3::Point2>> bldPts = loadPts(bldPathParam);
    std::vector<geolib3::Polygon2> initBlds = convertToBlds(bldPts);

    INFO() << "Loading roads polylines: " << roadPathParam;
    std::vector<std::vector<geolib3::Point2>> roadPts = loadPts(roadPathParam);
    std::vector<geolib3::Polyline2> roads = convertToRoads(roadPts);

    INFO() << "Aligning building polygons to roads";
    autocart::AlignAlongRoadsParams params;
    std::vector<geolib3::Polygon2> resultBlds =
            autocart::alignAlongRoads(initBlds, roads, params);

    if (!dumpInitialImageParam.empty()) {
        INFO() << "Dump initial image";
        cv::Mat canvas = drawObjects(imgPathParam, initBlds, roads);
        cv::imwrite(dumpInitialImageParam, canvas);
    }

    if (!dumpResultImageParam.empty()) {
        INFO() << "Dump result image";
        cv::Mat canvas = drawObjects(imgPathParam, resultBlds, roads);
        cv::imwrite(dumpResultImageParam, canvas);
    }

    if (!dumpResultBldParam.empty()) {
        INFO() << "Dump result polygons";
        dumpBlds(dumpResultBldParam, resultBlds);
    }

    return EXIT_SUCCESS;
}
catch (const maps::Exception& e) {
    INFO() << e;
    return EXIT_FAILURE;
}
catch (const std::exception& e) {
    INFO() << e.what();
    return EXIT_FAILURE;
}
catch (...) {
INFO() << "Caught unknown exception";
return EXIT_FAILURE;
}
